/*
lidar_navigation.c
提供AGV绕车控制功能
designed by linxiantao
--------------------------------
2020-05-07 15:20:00
create file
--------------------------------
*/
#include "../algorithm/con_algorithm.h"
#include "lidar_navigation.h"
#include "driver.h"
#include "agv.h"
#include "../driver/lidar/bea_u951.h"
#include "../driver/pansonic/wheel.h"
#include "../driver/lidar/linux_lidar.h"
#include "../driver/leisai-485/rs57.h"
#include "wheel.h"
#ifdef WASHER_MERGE
#include "../driver/wit-motion/wt901c485.h"
#endif
#include "joint.h"
#include "safty.h"

__weak int xp_wheel_set_wheel_speed(float line_speed0,float line_speed1){
	println("weak function~");
}
__weak int xp_wheel_get_wheel_locker(void){
	println("weak function~");
}
float get_mean_square_error(int32_t *lidar_distance, int32_t size, int32_t average);

#define ROBOT_CAR_SEARCH_VALUE 150//找到车的距离阈值，<150认为找到车，>150认为扫描不到车
#define LIDAR_DIST_MAX_VALUE 140//雷达算法输出的距离极限值
#define ROBOT_PID_TURN_TIMEOUT 100000
#define ROBOT_LOCAL_TURN_TIMEOUT 40000
#define ROBOT_ODOM_CALC_PERIOD 200//里程计计算周期，单位ms
#ifdef DRYER//吹风机使用更小的pid阈值，避免中间部分撞到车身
#define PID_INCREMENTAL_POS_THRESHOLD (0.05)//pid增量值正向阈值
#define PID_INCREMENTAL_NEG_THRESHOLD (-0.05)//pid增量值负向阈值
#else
#define PID_INCREMENTAL_POS_THRESHOLD (0.06)//pid增量值正向阈值
#define PID_INCREMENTAL_NEG_THRESHOLD (-0.06)//pid增量值负向阈值
#endif
#define U64_MAX_VAL (0xFFFFFFFFFFFFFFFFULL)
#define ROUND_LONG_SIDE_MIN_DIST (1600)//绕长边时的最小运行距离，单位mm
#define ROUND_SHORT_SIDE_MIN_DIST (100)//绕短边时的最小运行距离，单位mm
lidar_navigation_object lidar_navi;
static bool wash_car_roof_flag = 0;
static bool soft_reset_gyro_flag = 0;
static bool dist_filter_flag = 0;//异常雷达距离过滤标志
static bool leave_adjust_pose_flag = 0;
static aos_sem_t round_side_reset_gyro_sem;
static aos_sem_t round_side_dist_reset_gyro_sem;
static aos_sem_t robot_straightening_sem;
int32_t robot_straightening_side;
static aos_sem_t round_side_sem;
static bool b_round_test_end = false;
static aos_sem_t turning_sem;
static bool b_turning = false;
static PIDtypedef motion_pid;
static aos_sem_t start_odom_sem;
static aos_timer_t odom_calc_timer;
static float robot_angle_zero_offset = 0;
Robot_pose_object robot_odom_pose[4];//启用时应修改为4096
static float turn_angle = 20;
static float pid_turn_angle = 30;
int32_t round_side_reset_gyro_flag = 0;//找平复位陀螺仪完成标志
int32_t round_side_reset_gyro_timeout = 30000;//单位ms
int32_t round_side_reset_gyro_dist = 3000;//单位mm
int32_t dist_reset_gyro_round_side_end_flag = 0;//距离找平绕边结束标志
int32_t car_head_x_position = 0;	//车头的X坐标
int32_t car_tail_x_position = 0;	//车尾的X坐标
int32_t car_left_parallel_y = 0;	//左侧找平处车辆Y坐标
int32_t car_left_parallel_x = 0;	//左侧找平处车辆X坐标
int32_t car_right_parallel_y = 0;	//右侧找平处车辆Y坐标
int32_t car_right_parallel_x = 0;	//右侧找平处车辆X坐标

/**************************************/
//根据雷达检测调整位姿相关变量和宏定义
static aos_sem_t adjust_pose_by_lidar_sem;
static aos_sem_t leave_adjust_pose_by_lidar_sem;
int32_t adjust_pose_by_lidar_timeout = 60000;
bool adjust_pose_by_lidar_end_flag = false;
int32_t org_x2 = 292;
int32_t org_y2 = -322;
int32_t org_x1 = 62;
int32_t org_y1 = -167;
/*************************************/
static BEA_class bea2={0};
static aos_sem_t leave_sem;
static uint64_t FindCarMileage = 0;
static uint64_t FindCarDistance = 0;
bool isFindCarComplete = false;
static int isLeaveCar = 0;
static uint8_t leave_car_type = 0;
float last_pid_out = 0;
float f_K = 1.0f;
static float corner_move_s=0.1;
/**************************************/
//矩形转弯测试用
static uint16_t Pid_allow_range = 120;
static uint16_t Angle_1 = 45;
static uint16_t Angle_2 = 70;
static uint16_t Leave_car_dist = 900;
/*************************************/

//双机距离控制相关变量
#define PIPELINE_COLLISION_DIST_THRESHOLD 4000//单位mm
ROBOT_POSE another_robot_pose;
u8 another_robot_side;
static bool b_detect_pipe_collision = false;//是否检测到管线架碰撞
static bool pipe_collision_enable_flag = false;//是否使能管线架碰撞检测

int32_t robot_stop();
static int32_t robot_going();
static int32_t robot_going_line(float line_speed);
static int32_t robot_going_joint_move(bool b_new_point, Type_Vector3_Def tar_point);
bool is_angle_in_side_range(float robot_angle, int8_t target_side, float angle_range);

/**
 * @brief 设置洗车顶标志
 */
void set_wash_car_roof_flag(bool val)
{
	wash_car_roof_flag = val;
}

/**
 * @brief 返回洗车顶标志
 */
bool is_wash_car_roof_flag()
{
	return wash_car_roof_flag;
}

/**
 * @brief 设置陀螺仪软复位标志
 */
void set_soft_reset_gyro_flag(bool val)
{
	soft_reset_gyro_flag = val;
}

/**
 * @brief 返回陀螺仪软复位标志
 */
bool is_soft_reset_gyro_flag()
{
	return soft_reset_gyro_flag;
}

/**
 * @brief 设置异常雷达距离过滤标志
 */
void set_dist_filter_flag(bool val)
{
	dist_filter_flag = val;
}

/**
 * @brief 返回异常雷达距离是否要过滤
 */
bool is_dist_filter_flag()
{
	return dist_filter_flag;
}

/**
 * @brief 设置直行离车后再原地调整位姿标志
 */
void set_leave_adjust_pose_flag(bool val)
{
	leave_adjust_pose_flag = val;
	println("set_leave_adjust_pose_flag %d", val);
}

/**
 * @brief 返回直行离车后再原地调整位姿标志
 */
bool is_leave_adjust_pose_flag()
{
	return leave_adjust_pose_flag;
}

/**
 * @brief 设置管线架碰撞检查使能标志
 */
void set_pipe_collision_enable_flag(bool val)
{
	pipe_collision_enable_flag = val;
	println("set_pipe_collision_enable_flag %d", val);
}

/**
 * @brief 返回管线架碰撞检测使能标志
 */
bool is_pipe_collision_enable()
{
	return pipe_collision_enable_flag;
}

/**
 * @brief 设置汽车的车头x坐标
 */
void set_car_head_x_position(int32_t x_position)
{
	car_head_x_position = x_position;
	println("set_car_head_x_position %d", x_position);
}

/**
 * @brief 设置汽车的车尾x坐标
 */
void set_car_tail_x_position(int32_t x_position)
{
	car_tail_x_position = x_position;
	println("set_car_tail_x_position %d", x_position);
}

/**
 * @brief 获取汽车左侧找平处Y坐标
 */
int32_t get_car_left_parallel_y(void)
{
	return car_left_parallel_y;
}

/**
 * @brief 获取汽车左侧找平处X坐标
 */
int32_t get_car_left_parallel_x(void)
{
	return car_left_parallel_x;
}

/**
 * @brief 获取汽车右侧找平处Y坐标
 */
int32_t get_car_right_parallel_y(void)
{
	return car_right_parallel_y;
}

/**
 * @brief 获取汽车右侧找平处X坐标
 */
int32_t get_car_right_parallel_x(void)
{
	return car_right_parallel_x;
}

/**
 * @brief 机器人朝向角度软复位
 */
int robot_angle_soft_reset(int8_t side)
{
	uint8_t i = 0;
	float angle_buf[5] = 0;
	float angle_sum = 0;
	bool b_zero_pos = false;
	bool b_zero_neg = false;

	for(i = 0; i < 5; i++){
#ifdef ROBOT_ANGLE_USE_JOINT_ENCODER
		xp_joint_get_robot_pose(NULL, NULL, &angle_buf[i]);
#else
		angle_buf[i] = xp_wt901c_demo_zAngle_get();
#endif
		//识别角度是否在0度左右跳变
		if(angle_buf[i] > 0 && angle_buf[i] < 90){
			b_zero_pos = true;
		}
		if(angle_buf[i] > 270 && angle_buf[i] < 360){
			b_zero_neg = true;
		}
		aos_msleep(100);
	}

	//如果角度在0度左右跳变，需作特殊处理
	if(true == b_zero_pos && true == b_zero_neg){
		for(i = 0; i < 5; i++){
			if(angle_buf[i] > 270 && angle_buf[i] < 360){
				angle_buf[i] -= 360;
			}
		}
	}

	for(i = 0; i < 5; i++){
		angle_sum += angle_buf[i];
	}
	robot_angle_zero_offset = angle_sum / 5 - side * 90 % 360;
	println("robot_angle_soft_reset: side %d zero_offset is %.1f", side, robot_angle_zero_offset);
}

/**
 * @brief 获取机器人角度零位偏差，也即车辆停靠的偏角
 */
float get_robot_angle_zero_offset()
{
	return robot_angle_zero_offset;
}

/**
 * @brief 获取机器人朝向角，范围为逆时针[0, 360]，单位度
 * @return 机器人朝向角度，如果是361度，则说明获取不到，外部接口进行相应处理
 */
float get_robot_angle()
{
	int ret = 0;
	float angle_origin = 0;
	float angle_offset = 0;
	float robot_angle = 0;

#ifdef ROBOT_ANGLE_USE_JOINT_ENCODER
	ret = xp_joint_get_robot_pose(NULL, NULL, &angle_origin);
	if(ret != 0){
		//如果机器人位姿读取不到，则返回特殊值361度，用于外部接口判断
		return 361.0;
	}
#else
	angle_origin = xp_wt901c_demo_zAngle_get();
#endif
	angle_offset = robot_angle_zero_offset;
	robot_angle = fmod(angle_origin + 360 - angle_offset, 360);
	
	return robot_angle;
}
/**
 * @brief 获取通过里程计计算得到的朝向角度，单位度
 */
float get_odom_angle()
{
	float odom_angle = 0;
	
	if(xp_is_odom_enabled()){
		odom_angle = 360 - lidar_navi.odom.robot_angle_odom / 1000.0;
		//归一化到[0,360]范围内
		if(odom_angle > 360){
			odom_angle -= 360;
		}
	}else{
		odom_angle = 0;
	}

	return odom_angle;
}

/**
 * @brief 获取机器人位姿
 */
int32_t get_robot_pose(ROBOT_POSE *pose)
{
	int32_t ret = -1;
	int32_t p_x = 0, p_y = 0;
	float p_angle = 0;
	Odom_class *odom = xp_odom_object_get();

	if(NULL == pose){
		println("get_robot_pose: pose is NULL!");
		return -1;
	}
	
#ifdef ROBOT_ANGLE_USE_JOINT_ENCODER
	ret = xp_joint_get_robot_pose(&p_x, &p_y, &p_angle);
	if(ret != 0){
		println("xp_joint_get_robot_pose failed! ret %d", ret);
		return -2;
	}

	pose->p_x = p_x;
	pose->p_y = p_y;
	pose->p_th = (int32_t)(p_angle * 1000);
#else
	pose->p_x = odom->x;
	pose->p_y = odom->y;
	pose->p_th = odom->robot_angle;
#endif

	return 0;
}

int32_t update_another_robot_pose(ROBOT_POSE pose, u8 side)
{
	another_robot_pose = pose;
	another_robot_side = side;

	xp_joint_updata_another_robot_pose(another_robot_pose.p_x, another_robot_pose.p_y, another_robot_pose.p_th);
	return 0;
}

int32_t get_another_robot_pose(ROBOT_POSE *pose)
{
	*pose = another_robot_pose;
	return 0;
}

int32_t get_another_robot_side(CAR_SIDE_E *side)
{
	switch (another_robot_side)
	{
	case 0: *side = SIDE_DA;break;
	case 1: *side = SIDE_AB;break;
	case 2: *side = SIDE_BC;break;
	case 3: *side = SIDE_CD;break;
	default: *side = SIDE_MAX;break;
	}

	return 0;
}

/**
 * @brief 是否启用里程计
 */
uint8_t xp_is_odom_enabled()
{
	return lidar_navi.b_odom_enable;
}
/**
 * @brief 打开或关闭里程计
 */
void xp_odom_enable(uint8_t b_enable)
{
	lidar_navi.b_odom_enable = b_enable;
}
/**
 * @brief 里程计启动信号
 */
void xp_odom_start_signal()
{
	aos_sem_signal(&start_odom_sem);
}
/**
 * @brief 启动里程计
 */
void xp_odom_start()
{
	int ret = 0;
	uint64_t last_mileage = lidar_navi.odom.mileage_mm;
	
	memset(robot_odom_pose, 0, sizeof(robot_odom_pose));
	memset(&lidar_navi.odom, 0, sizeof(Odom_class));
	lidar_navi.odom.mileage_mm = last_mileage;
	xp_wheel_encode_get(&lidar_navi.odom.encode_last[WHEEL_L], &lidar_navi.odom.encode_last[WHEEL_R]);
	lidar_navi.odom.x = 4000;//起始点X偏移量
	lidar_navi.odom.y = 1000;//起始点Y偏移量
	lidar_navi.odom.robot_init_angle = (int32_t)(get_robot_angle()*1000);
	println("robot_init_angle is %d", lidar_navi.odom.robot_init_angle);
	
	ret = aos_timer_start(&odom_calc_timer);
	if (ret != 0 )
	{
		printf("start odom_calc_timer failed!\n");
	}
	else
	{
		printf("start odom_calc_timer success!\n");
	}
}
/**
 * @brief 停止里程计
 */
void xp_odom_stop()
{
	int ret = 0;
	ret = aos_timer_stop(&odom_calc_timer);
	if (ret != 0 )
	{
		printf("stop odom_calc_timer failed!\n");
	}
	else
	{
		printf("stop odom_calc_timer success!\n");
	}
}
/**
 * @brief 打印里程计数据
 */
void xp_odom_print()
{
	uint32_t i = 0;
	for (i = 0; i < sizeof(robot_odom_pose)/sizeof(Robot_pose_object); i++)
	{
		printf("%d,%d,%d,%d\r\n", robot_odom_pose[i].x, robot_odom_pose[i].y, robot_odom_pose[i].th_odom, robot_odom_pose[i].th_gyro);
	}
}
/**
 * @brief 里程计实时计算
 */
void xp_odom_calc(void *arg1, void *arg2)
{
	int32_t inc_encode[WHEEL_NUMS] = {0};
	float motor_delta_dist[WHEEL_NUMS];
	float delta_dist = 0.0;
	float delta_th = 0.0;
	float robot_vel = 0.0;
	float robot_vth = 0.0;
	float dx, dy;
	static ROBOT_POSE pose = {0};//获取失败时使用上次的值
	
	if (lidar_navi.odom.calc_cnt % 50 == 0)
	{
		printf("xp_odom_calc cnt %d\n", lidar_navi.odom.calc_cnt);
	}
	xp_wheel_encode_get(&lidar_navi.odom.encode_now[WHEEL_L], &lidar_navi.odom.encode_now[WHEEL_R]);
	//计算左轮编码器计数变化量
	if (abs(lidar_navi.odom.encode_now[WHEEL_L] - lidar_navi.odom.encode_last[WHEEL_L]) > (U64_MAX_VAL/2))
	{
		//左轮编码器数据翻转
		if (lidar_navi.odom.encode_now[WHEEL_L] < lidar_navi.odom.encode_last[WHEEL_L])
		{
			//左轮往前转
			inc_encode[WHEEL_L] = U64_MAX_VAL - lidar_navi.odom.encode_last[WHEEL_L] + lidar_navi.odom.encode_now[WHEEL_L];
		}
		else
		{
			//左轮往后转
			inc_encode[WHEEL_L] = -(U64_MAX_VAL - lidar_navi.odom.encode_now[WHEEL_L] + lidar_navi.odom.encode_last[WHEEL_L]);
		}
	}
	else
	{
		//左轮编码器数据未翻转
		if (lidar_navi.odom.encode_now[WHEEL_L] < lidar_navi.odom.encode_last[WHEEL_L])
		{
			//左轮往前转
			inc_encode[WHEEL_L] = lidar_navi.odom.encode_last[WHEEL_L] - lidar_navi.odom.encode_now[WHEEL_L];
		}
		else
		{
			//左轮往后转
			inc_encode[WHEEL_L] = -(lidar_navi.odom.encode_now[WHEEL_L] - lidar_navi.odom.encode_last[WHEEL_L]);
		}
	}
	//计算右轮编码器计数变化量
	if (abs(lidar_navi.odom.encode_now[WHEEL_R] - lidar_navi.odom.encode_last[WHEEL_R]) > (U64_MAX_VAL/2))
	{
		//右轮编码器数据翻转
		if (lidar_navi.odom.encode_now[WHEEL_R] < lidar_navi.odom.encode_last[WHEEL_R])
		{
			//右轮往前转
			inc_encode[WHEEL_R] = U64_MAX_VAL - lidar_navi.odom.encode_last[WHEEL_R] + lidar_navi.odom.encode_now[WHEEL_R];
		}
		else
		{
			//右轮往后转
			inc_encode[WHEEL_R] = -(U64_MAX_VAL - lidar_navi.odom.encode_now[WHEEL_R] + lidar_navi.odom.encode_last[WHEEL_R]);
		}
	}
	else
	{
		//右轮编码器数据未翻转
		if (lidar_navi.odom.encode_now[WHEEL_R] > lidar_navi.odom.encode_last[WHEEL_R])
		{
			//右轮往前转
			inc_encode[1] = lidar_navi.odom.encode_now[WHEEL_R] - lidar_navi.odom.encode_last[WHEEL_R];
		}
		else
		{
			//右轮往后转
			inc_encode[1] = -(lidar_navi.odom.encode_last[WHEEL_R] - lidar_navi.odom.encode_now[WHEEL_R]);
		}
	}
	//计算左右轮行走距离
	motor_delta_dist[WHEEL_L] = (float)inc_encode[WHEEL_L] / ((1<<23) * WHEEL_ratio) * (2 * PI * WHEEL_radius);
	motor_delta_dist[WHEEL_R] = (float)inc_encode[WHEEL_R] / ((1<<23) * WHEEL_ratio) * (2 * PI * WHEEL_radius);
	//以下计算基于机器人本体坐标系
	delta_dist = (motor_delta_dist[WHEEL_L] + motor_delta_dist[WHEEL_R]) / 2.0;
	delta_th = (motor_delta_dist[WHEEL_R] - motor_delta_dist[WHEEL_L]) / WHEEL_dist;
	robot_vel = delta_dist / ROBOT_ODOM_CALC_PERIOD;//单位 m/s
	robot_vth = 1000.0 * delta_th / ROBOT_ODOM_CALC_PERIOD;//单位 弧度/s
	lidar_navi.odom.vel_x = robot_vel;
	lidar_navi.odom.vel_y = 0;
	lidar_navi.odom.vel_z = robot_vth;
	dx = cos(delta_th) * delta_dist;
	dy = -sin(delta_th) * delta_dist;
	/*if (lidar_navi.odom.calc_cnt % 10 == 0){
		println("delta_dist %f delta_th %f, robot_vel %f robot_vth %f, vel %f %f %f, dx %f dy %f\n", 
			delta_dist, delta_th, robot_vel, robot_vth, 
			lidar_navi.odom.vel_x, lidar_navi.odom.vel_y, lidar_navi.odom.vel_z, dx, dy);
	}*/
	//x,y,z为世界坐标系
	lidar_navi.odom.x += cos(lidar_navi.odom.z) * dx - sin(lidar_navi.odom.z) * dy;
	lidar_navi.odom.y += sin(lidar_navi.odom.z) * dx + cos(lidar_navi.odom.z) * dy;
	lidar_navi.odom.z_odom += delta_th;
	lidar_navi.odom.robot_angle_odom = (int32_t)(180000 * lidar_navi.odom.z_odom / PI) % 360000;
	lidar_navi.odom.robot_angle = (360000 + (int32_t)(get_robot_angle()*1000) - lidar_navi.odom.robot_init_angle) % 360000;
	lidar_navi.odom.z = (float)lidar_navi.odom.robot_angle * PI / 180000.0;
	lidar_navi.odom.mileage_mm += abs(delta_dist) + 0.5;//四舍五入
	//println("x %f y %f z %f\n", lidar_navi.odom.x, lidar_navi.odom.y, lidar_navi.odom.z);
	
	//机器人位姿上传给业务主板
#ifdef ROBOT_ANGLE_USE_JOINT_ENCODER
	get_robot_pose(&pose);
	xp_upper_set_robot_pose(pose.p_th, pose.p_x, pose.p_y);
#else
	xp_upper_set_robot_pose(lidar_navi.odom.robot_angle,lidar_navi.odom.x,lidar_navi.odom.y);
#endif
	
	/*if (lidar_navi.odom.calc_cnt % 10 == 0){
		println("encode_now %d %d, encode_last %d %d, inc_encode %d %d, motor_delta_dist %f %f\n", 
			lidar_navi.odom.encode_now[0], lidar_navi.odom.encode_now[1],
			lidar_navi.odom.encode_last[0], lidar_navi.odom.encode_last[1],
			inc_encode[0], inc_encode[1], motor_delta_dist[0], motor_delta_dist[1]);
	}*/
	lidar_navi.odom.encode_last[WHEEL_L] = lidar_navi.odom.encode_now[WHEEL_L];
	lidar_navi.odom.encode_last[WHEEL_R] = lidar_navi.odom.encode_now[WHEEL_R];
#if 0
	/* 将每次计算的里程计数据保存到内存中，结束后打印分析 */
	if (lidar_navi.odom.calc_cnt < sizeof(robot_pose)/sizeof(Robot_pose_object))
	{
		robot_odom_pose[lidar_navi.odom.calc_cnt].x = (int)(lidar_navi.odom.x);
		robot_odom_pose[lidar_navi.odom.calc_cnt].y = (int)(lidar_navi.odom.y);
		robot_odom_pose[lidar_navi.odom.calc_cnt].th_odom = (int)(lidar_navi.odom.z_odom * 180 * 1000 / PI) % 360000;//单位1/1000度
		robot_odom_pose[lidar_navi.odom.calc_cnt].th_gyro = lidar_navi.odom.robot_angle;
	}
	else
	{
		xp_odom_stop();
	}
#endif
	lidar_navi.odom.calc_cnt++;
}
/**
 * @brief 获取里程距离，单位mm
 */
uint64_t xp_odom_get_distance(void)
{
	return lidar_navi.odom.mileage_mm;
}
/**
 * @brief 获取里程计对象
 */
Odom_class *xp_odom_object_get(void)
{
	return &lidar_navi.odom;
}
/**
 * @brief 里程计测试接口
 */
static void navi_odom_test(uint32_t test_dir)
{
	uint64_t encode_start[2] = {0};
	uint64_t encode_end[2] = {0};
	int32_t inc_encode[2] = {0};
	float motor_delta_dist[2];
	float delta_dist = 0.0;
	float delta_th = 0.0;
	float robot_vel = 0.0;
	float robot_vth = 0.0;
	float vel_x, vel_y, vel_z, dx, dy;
	static float x, y, z;
	
	xp_gyro_safe_reset();
	xp_wheel_encode_get(&encode_start[0], &encode_start[1]);
	if (1 == test_dir)
	{
		xp_wheel_set_wheel_speed(0.1, 0.1);
	}
	else
	{
		xp_wheel_set_wheel_speed(-0.1, -0.1);
	}
	aos_msleep(2000);
	xp_wheel_set_wheel_speed(0, 0);
	xp_wheel_encode_get(&encode_end[0], &encode_end[1]);
	if (abs(encode_end[0] - encode_start[0]) > (U64_MAX_VAL/2))
	{
		//左轮编码器数据翻转
	}
	else
	{
		if (encode_end[0] < encode_start[0])
		{
			//左轮往前转
			inc_encode[0] = (int32_t)(encode_start[0] - encode_end[0]);
		}
		else
		{
			//左轮往后转
			inc_encode[0] = -(int32_t)(encode_end[0] - encode_start[0]);
		}
	}
	if (abs(encode_end[1] - encode_start[1]) > (U64_MAX_VAL/2))
	{
		//右轮编码器数据翻转
	}
	else
	{
		if (encode_end[1] > encode_start[1])
		{
			//右轮往前转
			inc_encode[1] = (int32_t)(encode_end[1] - encode_start[1]);
		}
		else
		{
			//右轮往后转
			inc_encode[1] = -(int32_t)(encode_start[1] - encode_end[1]);
		}
	}
	motor_delta_dist[0] = (float)inc_encode[0] / ((1<<23) * WHEEL_ratio) * (2 * PI * WHEEL_radius);
	motor_delta_dist[1] = (float)inc_encode[1] / ((1<<23) * WHEEL_ratio) * (2 * PI * WHEEL_radius);
	println("encode_start %llu %llu, encode_end %llu %llu, inc_encode %d %d\n", 
			encode_start[0], encode_start[1],
			encode_end[0], encode_end[1],
			inc_encode[0], inc_encode[1]);
	println("motor_delta_dist %f %f\n", motor_delta_dist[0], motor_delta_dist[1]);
	//以下计算基于机器人本体坐标系
	delta_dist = (motor_delta_dist[0] + motor_delta_dist[1]) / 2.0;
	delta_th = (motor_delta_dist[1] - motor_delta_dist[0]) / WHEEL_dist;
	robot_vel = delta_dist / 2000;
	robot_vth = 1000.0 * delta_th / 2000;
	vel_x = robot_vel;
	vel_y = 0;
	vel_z = robot_vth;
	dx = cos(delta_th) * delta_dist;
	dy = -sin(delta_th) * delta_dist;
	println("delta_dist %f delta_th %f, robot_vel %f robot_vth %f, vel %f %f %f, dx %f dy %f\n", 
			delta_dist, delta_th, robot_vel, robot_vth, vel_x, vel_y, vel_z, dx, dy);
	//x,y,z为世界坐标系
	x += cos(z) * dx - sin(z) * dy;
	y += sin(z) * dx + cos(z) * dy;
	z += delta_th;
	println("x %f y %f z %f, robot_angle %f\n", x, y, z, get_robot_angle());
}
/**
 * @brief 里程计测试
 */
int xp_odom_test(char *type,char *fun,char *param){
    if(strcmp(type,"odom")!=0)return 0;
    if(strcmp(fun,"test")==0){
		uint32_t test_dir = atoi(param);
		navi_odom_test(test_dir);
    }
	else if(strcmp(fun,"open")==0){
		xp_odom_enable(1);
    }
	else if(strcmp(fun,"close")==0){
		xp_odom_enable(0);
    }
	else if(strcmp(fun,"print")==0){
		xp_odom_print();
    }
	else if(strcmp(fun,"start")==0){
		xp_gyro_safe_reset();
		xp_odom_start_signal();
    }
	else if(strcmp(fun,"stop")==0){
		xp_odom_stop();
    }
    else{
        return 0;
    }
    return 1;
}

/**
 * @brief 原地转弯测试
 * @param turn_angle 转动角度，单位1/1000度
 * @return 0-转弯成功，1-提前转弯结束，<0-转弯失败
 */
int32_t xp_local_turn(int32_t turn_angle)
{
	int32_t ret = 0;
	float robot_angle = 0;
	int32_t start_angle = 0;
	int32_t cur_angle = 0;
	int32_t angle_devi = 0;
	int32_t lidar_dist = 0;
	uint8_t retry_cnt = 0;
	uint8_t near_cnt = 0;
	float speed_l = 0;
	float speed_r = 0;
	float pid_out = 0.0;
	long long start_time = 0, cur_time = 0;
	bool b_early_stop = false;

	//获取初始时刻和初始角度
	start_time = aos_now_ms();
	do{
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		robot_angle = get_robot_angle();
		if(robot_angle > 360){
			println("local_turn: robot_angle(%f) get failed! Retry %d", robot_angle, ++retry_cnt);
			if(retry_cnt < 10){
				aos_msleep(500);
				continue;
			}else{
				println("local_turn: get angle failed!");
				return -1;
			}
		}else{
			break;
		}
	}while(1);
	start_angle = (int32_t)(robot_angle * 1000);

	//设置PID参数和目标
	memset(&motion_pid, 0, sizeof(motion_pid));
	motion_pid.Kp = 0.000008;
	motion_pid.Ki = 0.000005;
	motion_pid.setpoint = turn_angle;
	
	while (abs(angle_devi - turn_angle) > 2000)
	{
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		cur_time = aos_now_ms();
		if(cur_time - start_time > ROBOT_LOCAL_TURN_TIMEOUT){
			println("xp_local_turn angle %d timeout! start %lld, cur %lld", turn_angle, start_time, cur_time);
			return -2;
		}

#if xp_config_disabled(WASH_ROOF_RECTANGULAR)
		//如果逆时针转弯过程中雷达距离小于90cm，则提前退出，避免撞车
		if(turn_angle > 0){
			lidar_dist = xp_navi_lidar_get2();
			if(lidar_dist < 90){
				near_cnt++;
				if(near_cnt > 8){
					println("xp_local_turn: lidar_dist %d < 90! stop immediately!", lidar_dist);
					b_early_stop = true;
					break;
				}
			}else{
				near_cnt = 0;
			}
		}
#endif
		robot_angle = get_robot_angle();
		if(robot_angle > 360){
			robot_stop();
			println("local_turn: robot_angle(%f) get failed!", robot_angle);
			continue;
		}
		cur_angle = (int32_t)(robot_angle * 1000);
		if((cur_angle >= 0 && cur_angle < 90000) && (start_angle > 270000 && start_angle < 360000)){
			//当前角度在360度左侧，起始角度在360度右侧
			angle_devi = (cur_angle - start_angle + 360000) % 360000;
		}else if((cur_angle > 270000 && cur_angle < 360000) && (start_angle >= 0 && start_angle < 90000)){
			//当前角度在360度右侧，起始角度在360度左侧
			angle_devi = (cur_angle - start_angle - 360000) % 360000;
		}else{
			angle_devi = (cur_angle - start_angle) % 360000;
		}
		pid_out = PIDx_calc(&motion_pid, angle_devi);
		if(pid_out > 0.15){
			pid_out = 0.15;
		}
		if(pid_out < -0.15){
			pid_out = -0.15;
		}
		speed_l = -pid_out;
		speed_r = pid_out;
		println("turn_angle %d angle_devi %d-%d=%d, pid_inc %.3f, speed %.3f %.3f", turn_angle, cur_angle, start_angle, angle_devi, pid_out, speed_l, speed_r);
		if (speed_l > 0.2)
		{
			speed_l = 0.2;
		}
		else if (speed_l < -0.2)
		{
			speed_l = -0.2;
		}
		if (speed_r > 0.2)
		{
			speed_r = 0.2;
		}
		else if (speed_r < -0.2)
		{
			speed_r = -0.2;
		}
		ret = xp_wheel_set_wheel_speed(speed_l,speed_r);
		if(0 != ret){
			println("xp_wheel_set_wheel_speed failed! ret is %d\n", ret);
			//return -1;
		}
		aos_msleep(100);
	}

	robot_stop();
	aos_msleep(1000);
	// angle_devi = (int32_t)(get_robot_angle()*1000) - start_angle;
	cur_angle = (int32_t)(robot_angle * 1000);
	if((cur_angle >= 0 && cur_angle < 90000) && (start_angle > 270000 && start_angle < 360000)){
		//当前角度在360度左侧，起始角度在360度右侧
		angle_devi = (cur_angle - start_angle + 360000) % 360000;
	}else if((cur_angle > 270000 && cur_angle < 360000) && (start_angle >= 0 && start_angle < 90000)){
		//当前角度在360度右侧，起始角度在360度左侧
		angle_devi = (cur_angle - start_angle - 360000) % 360000;
	}else{
		angle_devi = (cur_angle - start_angle) % 360000;
	}
	println("turn finish: turn_angle %d, actually angle_devi %d", turn_angle, angle_devi);

	if(true == b_early_stop){
		return 1;
	}else{
		return 0;
	}
}
/**
 * @brief 直线运动到目标点测试
 */
int32_t xp_straight_test(int32_t x_goal, int32_t y_goal, uint8_t dir)
{
	int32_t ret = 0;
	int32_t x = 0, y = 0;
	float theta = 0;
	float theta_goal = 0;
	float theta_err = 0;
	int32_t dist_err = 0;
	float speed_cmd = 0.1;
	float speed_l = 0;
	float speed_r = 0;
	float pid_out = 0.0;

	//启动里程计
	xp_odom_start_signal();
	aos_msleep(2000);

	//设置PID参数和目标
	memset(&motion_pid, 0, sizeof(motion_pid));
	motion_pid.Kp = 0.1;
	motion_pid.Ki = 0.05;
	motion_pid.setpoint = 0;
	
	x = (int32_t)(lidar_navi.odom.x);
	y = (int32_t)(lidar_navi.odom.y);
	dist_err = (int32_t)sqrt((float)(x_goal-x)*(x_goal-x) + (float)(y_goal-y)*(y_goal-y));
	while(dist_err > 200)
	{
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		//计算当前与目标的朝向误差
		theta = lidar_navi.odom.z_odom;
		theta_goal = atan((float)(y_goal-y) / (float)(x_goal-x));
		theta_err = theta - theta_goal;
		pid_out = PIDx_calc(&motion_pid, theta_err);
		speed_l = speed_cmd - pid_out;
		speed_r = speed_cmd + pid_out;
		println("dist_err %d theta_err %.3f - %.3f = %.3f, pid_out %f, speed %f %f", 
			dist_err, theta, theta_goal, theta_err, pid_out, speed_l, speed_r);
		if (speed_l > 0.2)
		{
			speed_l = 0.2;
		}
		else if (speed_l < -0.2)
		{
			speed_l = -0.2;
		}
		if (speed_r > 0.2)
		{
			speed_r = 0.2;
		}
		else if (speed_r < -0.2)
		{
			speed_r = -0.2;
		}
		ret = xp_wheel_set_wheel_speed(speed_l,speed_r);
		if(0 != ret){
			println("xp_wheel_set_wheel_speed failed! ret is %d\n", ret);
			//return -1;
		}
		aos_msleep(50);

		x = (int32_t)(lidar_navi.odom.x);
		y = (int32_t)(lidar_navi.odom.y);
		dist_err = (int32_t)sqrt((float)(x_goal-x)*(x_goal-x) + (float)(y_goal-y)*(y_goal-y));
	}

	robot_stop();
	aos_msleep(1000);
	x = (int32_t)(lidar_navi.odom.x);
	y = (int32_t)(lidar_navi.odom.y);
	dist_err = (int32_t)sqrt((float)(x_goal-x)*(x_goal-x) + (float)(y_goal-y)*(y_goal-y));
	println("straight finish: dist_err %d, robot_angle %d", dist_err, lidar_navi.odom.robot_angle_odom);

	//关闭里程计
	xp_odom_stop();

	return 0;
}
/**
 * @brief 逆时针90°弧线转弯测试
 */
int32_t xp_curve_test(int32_t radius)
{
	float angle_rate = PI / 4 / 5.0;//5s完成90°圆弧转弯
	float base_speed = 0.1;
	float speed_l = 0;
	float speed_r = 0;
	int32_t ret = 0;
	speed_l = (radius - WHEEL_dist/2) * angle_rate / 1000.0;
	speed_r = (radius + WHEEL_dist/2) * angle_rate / 1000.0;
	println("radius %d angle_rate %f, speed %f %f", radius, angle_rate, speed_l, speed_r);
	
	ret = xp_wheel_set_wheel_speed(speed_l,speed_r);
	if(0 != ret){
		println("xp_wheel_set_wheel_speed failed! ret is %d\n", ret);
		return -1;
	}
	aos_msleep(5000);
	ret = xp_wheel_set_wheel_speed(0,0);
	if(0 != ret){
		println("xp_wheel_set_wheel_speed failed! ret is %d\n", ret);
		return -1;
	}
	return 0;
}
/**
 * @brief 运动控制测试
 */
int xp_motion_test(char *type,char *fun,char *param){
	if(strcmp(type,"motion")!=0)return 0;
    if(strcmp(fun,"turn")==0){
		//原地转弯相对角度，逆时针为正，顺时针为负
		int32_t turn_angle = atoi(param);
		xp_local_turn(turn_angle);
    }
	else if(strcmp(fun,"forward")==0){
		char *p1, *p2;
		int32_t x_goal = 0, y_goal = 0;
		p1 = strtok_r(param, ",", &p2);
		println("p1 %s, p2 %s", p1, p2);
		x_goal = atoi(p1);
		y_goal = atoi(p2);
		println("robot forword goal: (%d,%d)", x_goal, y_goal);
		xp_straight_test(x_goal, y_goal, 1);
    }
	else if(strcmp(fun,"backward")==0){
		//xp_straight_test(0);
    }
	else if(strcmp(fun,"curve")==0){
		//弧线转弯半径，单位mm，默认逆时针转90°
		int32_t curve_radius = atoi(param);
		xp_curve_test(curve_radius);
    }
    else{
        return 0;
    }
    return 1;
}
void xp_motion_init(void)
{
	memset(&motion_pid, 0, sizeof(motion_pid));
	motion_pid.Kp = 0.000008;
	motion_pid.Ki = 0.000005;
}
/**
 * @brief 设置机器人导航的pid阈值
 * 
 * @param distance 机器人离小车的距离。
 */
void xp_set_robot_safe_distance(int32_t distance)
{
	lidar_navi.robot_safe_distance = distance;
	PIDx_setvalue(&(lidar_navi.pid), distance);
}
/*
get lidar distance cm，3D雷达视野最大，横向范围[-6cm,50cm]，垂直范围[0cm,160cm]（输出限制在[75cm,140cm]），适用于绕边
return: min distance cm
*/
int xp_navi_lidar_get(void){
	static int last_dist = 255;
	float robot_angle = get_robot_angle();
	int dis=xp_linux_lidar_dis_get();
	static u8 comfirm_valid_value_cnt = 0;
	static bool print_flag = false;

//	return get_lidar_distance();
	if(dis==255){
		
	}

	if(is_dist_filter_flag()){
		if(comfirm_valid_value_cnt < 5){			//启用滤波前连续多次判断当前值是否有效（连续多个距离相差不大认为有效）
			if(abs(last_dist - dis) < 5){
				comfirm_valid_value_cnt++;
			}else{
				comfirm_valid_value_cnt = 0;
			}
			last_dist = dis;
			return dis;
		}else{
			if(print_flag == false){
				println("identify valid value, start dis_filter");
				print_flag = true;
			}
		}

		//异常雷达距离需要过滤时，返回之前正常的雷达距离，避免在长边上异常拐弯（当前机器角度在与车平齐的误差范围内，若雷达距离过大或过小，则滤除）
		if(((dis > (lidar_navi.robot_safe_distance + 15)) || (dis < (lidar_navi.robot_safe_distance - 15))) && 
			(true == is_angle_in_side_range(robot_angle, lidar_navi.robot_round_side, 10.0)) && (last_dist != 160))
		{
			println("dist_filter: dist %d last_dist %d safe_dist %d", dis, last_dist, lidar_navi.robot_safe_distance);
			return last_dist;
		}else{
			last_dist = dis;
		}
	}else{
		print_flag = false;
		comfirm_valid_value_cnt = 0;
		last_dist = dis;
	}

	return dis;
}
/*
get lidar distance cm，3D雷达视野较小，横向范围[0cm,5cm]，垂直范围[0cm,160cm]，输出范围[0cm,160cm]，适用于后退、旋转等需要小视野的场景
return: min distance cm
*/
int xp_navi_lidar_get2(void){
	int dis=xp_linux_lidar_dis2_get();
//	return get_lidar_distance();
	if(dis==255){
		
	}
	return dis;
}
/*
get lidar distance cm，3D雷达视野只关注行进方向，横向范围[0cm,25cm]，垂直范围[0cm,65cm]，适用于避障
return: min distance cm
*/
int xp_navi_lidar_get3(void){
	int dis=xp_linux_lidar_dis3_get();
//	return get_lidar_distance();
	if(dis==255){
		
	}
	return dis;
}

/*
get lidar open status
return:
	1==normal
	0=error
*/
int xp_navi_lidar_status(void){
	
//	return is_lidar_open()
	if(xp_linux_lidar_dis_get()==255){
		return 0;
	}
	else{
		return 1;
	}
}
/**
 * @brief 判断是否时拐弯点
 *  
 * @param side具体哪一条边的拐弯点
 * @return true表示是到拐弯点了，false则不是。
 */
static bool get_turn_flag(int8_t side)
{
	int32_t ret = 0;
	int32_t x = 0;
	float robot_angle = 0.0;
	static uint8_t cnt = 0;
	
	robot_angle = get_robot_angle();
	if(robot_angle > 360){
		println("get_turn_flag: robot_angle(%f) get failed!", robot_angle);
		return false;
	}

	//洗车机洗左侧车顶时，绕边结束角度为14°，其它情况为20°
	if((2 == (side % 4)) && (true == is_wash_car_roof_flag()))
	{
		turn_angle = 14;
		println("now side %d identify turn angle is %0.3f", side, turn_angle);
	}
	else
	{
		turn_angle = 20;
	}

	if(side == 0 + 4 * 4){
		//洗车机DA边空跑时需要提前绕边结束，之后再移动到离车检测位置
		ret = xp_joint_get_robot_pose(&x, NULL, NULL);
		if(0 == ret){
			if(x > 1200){
				println("get_turn_flag: x %d > 1200", x);
				cnt++;
				if(cnt > 3){
					ret = robot_stop();
					if(0 != ret){
						println("robot stop error! ret %d", ret);
						xp_upper_navi_state_set(1);
					}
					return true;
				}
			}else{
				cnt = 0;
			}
		}else{
			return false;
		}
	}else{
		//往里偏turn_angle°认为是拐弯点
		if((robot_angle > ((side % 4) * 90 + turn_angle))
			&& (robot_angle < ((side % 4 + 1) * 90)))
		{
			cnt++;
			if(cnt > 3){
				ret = robot_stop();
				if(0 != ret){
					println("robot stop error! ret %d", ret);
					xp_upper_navi_state_set(1);
				}
				return true;
			}
		}
		else
		{
			cnt = 0;
			return false;
		}
	}

	return false;
}
/**
 * @brief 机器人是否找到车头了
 * 
 * @return bool 0表示机器人找到车头了，非0则表示机器人还没找到车头。
 */
bool xp_is_robot_search_car()
{
	int32_t ret;
	int32_t distance;
	static u8 count=0;
	
	distance = xp_navi_lidar_get();
	if(0 != distance && distance < ROBOT_CAR_SEARCH_VALUE)
	{
		count++;
		if(count>5){
			count=0;
			return 1;
		}
	}
	else{
		count=0;
	}
	return 0;
}
/**
 * @brief 机器人绕车一边是否完成
 * 
 * @return int32_t 1表示绕车一边完成，非0则表示未完成。
 */
int32_t xp_is_robot_round_side_reset_gyro_end()
{
	return round_side_reset_gyro_flag;
}
void xp_reset_robot_round_side_reset_gyro()
{
	round_side_reset_gyro_flag = 0;
}
int32_t xp_is_robot_dist_reset_gyro_round_side_end()
{
	return dist_reset_gyro_round_side_end_flag;
}
void xp_reset_robot_dist_reset_gyro_round_side_end()
{
	dist_reset_gyro_round_side_end_flag = 0;
}

/**
 * @brief 机器人pid行走一段时间
 * 
 * @param time pid绕行的超时时间，单位ms
 */
int32_t xp_robot_round_side_time_reset_gyro(int32_t timeout)
{
	int32_t ret = 0;
	long long last_time = 0, cur_time = 0;
	int i = 0;
	float robot_angle = 0.0;
	int l=0, safty=0;
	int short_cnt = 0;
	int mid_cnt = 0;
	int long_cnt = 0;
	uint64_t start_dist = 0;
	
	last_time = aos_now_ms();
	cur_time = aos_now_ms();
	//移动前先复位绕边行走结束标志
	xp_reset_robot_round_side();
	//开始移动前启动里程计——已经在复位陀螺仪时启动
	//xp_odom_start_signal();
	//设置pid绕行速度
	set_robot_base_speed(0.15);
	//开始直线往前移动
	robot_front(0.15);
	start_dist = xp_odom_get_distance();
	
	while(1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		l = xp_navi_lidar_get();
		
		if(cur_time - last_time > timeout){
			robot_stop();
			//lidar_navi.robot_round_side_end = 1;
			xp_upper_navi_state_set(1);
			println("robot_round_side_reset_gyro timeout(%d) in first stage!!!", timeout);
			return -1;
		}
		
		if(l<85 && l>60){
			mid_cnt++;
			short_cnt = 0;
			if(mid_cnt > 10){
				break;
			}
		}
		else if(l<140 && l>85){
			long_cnt++;
			short_cnt = 0;
			mid_cnt = 0;
			if(long_cnt > 40){
				break;
			}
		}
		else if(l>0 && l<60){
			if(short_cnt < 10){
				short_cnt++;
			}
			if(short_cnt == 10){
				short_cnt++;
				robot_stop();
				safty=1;
			}
		}
		else{
			short_cnt = 0;
			mid_cnt = 0;
			long_cnt = 0;
		}
		
		if(safty==1 && (l>60||l==0)){
			safty = 0;
			robot_front(0.15);
		}
		aos_msleep(100);
		cur_time = aos_now_ms();
	}
	
	while(cur_time - last_time < timeout){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		//找平后进行处理，否则继续找
		if(true == is_find_car_parallel())
		{
			robot_stop();
			aos_msleep(3000);
			FindCarMileage = xp_odom_get_distance();
			FindCarDistance = FindCarMileage - start_dist;
			isFindCarComplete = true;
			println("find car dis %lld", FindCarDistance);
			//机器人朝向角度软复位，并重新启动里程计
			robot_angle_soft_reset(0);
			xp_odom_start_signal();
			aos_msleep(1000);
			round_side_reset_gyro_flag = 1;
			lidar_navi.robot_round_side_end = 1;
			return 0;
		}
		
		ret = robot_going();
		if(0 != ret){
			robot_stop();
			println("robot going error! ret %d", ret);
			xp_upper_navi_state_set(1);
		
			return -1;
		}
		cur_time = aos_now_ms();
		aos_msleep(100);
	}
	
	ret = robot_stop();
	if(0 != ret){
		println("robot stop error! ret %d", ret);
		xp_upper_navi_state_set(1);
		
		return -1;
	}
	
	round_side_reset_gyro_flag = 1;
	lidar_navi.robot_round_side_end = 1;
	return 0;
}


/**
 * @brief 机器人pid行走一段距离再进行找平
 * 
 * @param dist 找平前pid绕行距离，单位mm
 */
int32_t xp_robot_round_side_dist_reset_gyro(int32_t dist)
{
	int32_t ret = 0;
	long long start_time = 0, cur_time = 0;
	int i = 0;
	float robot_angle = 0.0;
	bool turn_flag = false;
	uint64_t start_dist = 0;
	uint64_t diff_dist = 0;
	int err_cnt = 0;
	
	start_time = aos_now_ms();
	cur_time = aos_now_ms();

	//开始移动前启动里程计——已经在复位陀螺仪时启动
	//xp_odom_start_signal();
	//设置pid绕行速度
	set_robot_base_speed(0.15);
	start_dist = xp_odom_get_distance();
	
	while(1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		diff_dist = xp_odom_get_distance() - start_dist;

		//超过5分钟未找车平行也未达到车尾处
		if(cur_time - start_time > 300000){
			robot_stop();
			xp_upper_navi_state_set(1);
			println("robot_round_side_dist_reset_gyro timeout! start_time %lld cur_time %lld", start_time, cur_time);
			return -1;
		}
		
		turn_flag = get_turn_flag(2);
		if(turn_flag){
			//如果绕边直到车尾左后方都未找平，则直接认为结束
			println("dist_reset_gyro: turn_flag %d, side 2, diff_dist %lld", turn_flag, diff_dist);
			robot_stop();
			aos_msleep(1000);
			round_side_reset_gyro_flag = 1;
			dist_reset_gyro_round_side_end_flag = 1;
			return 0;
		}

		if(diff_dist > dist){
			//找平后进行处理，否则继续找
			if(true == is_find_car_parallel())
			{
				robot_stop();
				aos_msleep(3000);
				println("dist_reset_gyro: find car diff_dist %lld", diff_dist);
				//机器人朝向角度软复位，并重新启动里程计
				robot_angle_soft_reset(2);
				xp_odom_start_signal();
				aos_msleep(1000);
				round_side_reset_gyro_flag = 1;
				return 0;
			}
		}
		
		ret = robot_going();
		if(0 != ret){
			err_cnt++;
			if(err_cnt > 10){
				robot_stop();
				println("robot going error! ret %d", ret);
				xp_upper_navi_state_set(1);
				return -2;
			}
		}else{
			err_cnt = 0;
		}
		
		cur_time = aos_now_ms();
		aos_msleep(100);
	}
	
	return 0;
}

int32_t xp_robot_round_side_time_reset_gyro_test(int32_t timeout)
{
	#define MAX_RESET_GYRO_DISTENCE		4000	//复位陀螺仪最大距离,超出此距离时采用陀螺仪均值做偏移复位(mm)
	#define SAMPLING_WINDOW				20		//采样计算窗口(雷达点数或陀螺仪角度数)
	#define SAMPLING_STEP				30		//采样步进距离(mm)
	#define ALLOW_ERROR					3		//均方差允许误差

	uint64_t last_dist = xp_odom_get_distance();
	uint64_t start_dist = xp_odom_get_distance();

	uint8_t index = 0;
	int32_t dist_buf[SAMPLING_WINDOW];
	float angle_buf[SAMPLING_WINDOW];

	//lidar_navi.robot_round_side_end = 1;	//由于是洗车绕边的同时在找车,业务逻辑需要判断此条件才往下执行

	while(xp_odom_get_distance() - start_dist < MAX_RESET_GYRO_DISTENCE){	//限制陀螺仪复位的最远距离
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		if (xp_odom_get_distance() - last_dist > SAMPLING_STEP){	//根据里程计每一个步进距离进行一次采样
			last_dist = xp_odom_get_distance();
			if (index < SAMPLING_WINDOW){							//初始时填充滑动窗口数组
				dist_buf[index] = xp_navi_lidar_get();
				angle_buf[index] = get_robot_angle();
				index++;
				print("Full dist:%d angle:%f\r\n", dist_buf[index - 1], angle_buf[index - 1]);
			}else{
				for (uint8_t i = 0; i < SAMPLING_WINDOW - 1; i++){	//滑动窗口
					dist_buf[i] = dist_buf[i + 1];
					angle_buf[i] = angle_buf[i + 1];
				}
				dist_buf[SAMPLING_WINDOW - 1] = xp_navi_lidar_get();
				angle_buf[SAMPLING_WINDOW - 1] = get_robot_angle();
				print("Full dist:%d gyro:%f\r\n", dist_buf[SAMPLING_WINDOW - 1], angle_buf[SAMPLING_WINDOW - 1]);
				float err;
				if ((err = get_mean_square_error(dist_buf, SAMPLING_WINDOW, lidar_navi.robot_safe_distance)) <= ALLOW_ERROR){
					print("Gyro reset successful <user io> %f\r\n", err);
#if 0	//这里先不启用,测试稳定用
					xp_gyro_reset();
					round_side_reset_gyro_flag = 1;
#endif
					return 0;
				}
				print("err:%f\r\n", err);
			}
		}
		aos_msleep(100);
	}
	print("Used lidar reset gyro failed. start:%d End:%d\r\n", start_dist, xp_odom_get_distance());
	//超过最大的陀螺仪复位距离后雷达距离均方差仍未达到复位条件,这里采用陀螺仪的均值复位
	float sum = 0;
	for (uint8_t i = 0; i < SAMPLING_WINDOW; i++){
		sum += angle_buf[i];
	}
	sum /= SAMPLING_WINDOW;
	print("Gyro reset successful <user soft> %f\r\n", sum);
#if 0
	g_angle_zero_offset = sum;
	round_side_reset_gyro_flag = 1;
#endif
	return 0;
}
/**
 * @brief 机器人是否离开车
 * 
 * @return bool 0表示机器人离开车了，非0则表示机器人还没离开车。
 */
bool xp_robot_leave_car_B_D()
{
	int32_t ret;
	int32_t distance;
	uint64_t leave_start_dist = 0;
	uint8_t count = 0;
	uint8_t comfirm_leave_cnt = 0;
	uint8_t comfirm_not_leave_cnt = 0;
	bool b_lidar_leave = false;
	bool judge_leave_car = false;
	int32_t x, y;
	u64 start_time = 0,now_time = 0;
	
	isLeaveCar = 0;
	ret = xp_wheel_set_wheel_speed(-0.2, -0.2);
	if(0 != ret){
		robot_stop();
		return -1;
	}

	start_time = aos_now_ms();

#if xp_config_enabled(WASH_ROOF_RECTANGULAR)
	robot_stop();
	println("leave car: odom leave! start %lld now %lld pose[%d, %d]", leave_start_dist, xp_odom_get_distance(),x,y);
	isLeaveCar = 1;
	return 1;
#endif
	while (1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		now_time = aos_now_ms();
		distance = xp_navi_lidar_get2();

		if(false == judge_leave_car){
			//超过电子围栏则判定为已离车
			ret = xp_joint_get_efence_state();
			if(ret != 0){
				robot_stop();
				println("leave car out of range, force stop !!");
				judge_leave_car = true;
			}
			else{
				if (false == b_lidar_leave){
					if((distance > ROBOT_CAR_SEARCH_VALUE) && (distance != 255)){
						//检测到离开车的条件
						count++;
					}else if (0 != count){
						count = 0;
					}
					
					if (count == 2){
						ret = xp_wheel_set_wheel_speed(-0.08, -0.08);
						if(0 != ret){
							robot_stop();
							return -1;
						}
					}
					
					if (count >= 8){
						leave_start_dist = xp_odom_get_distance();
						b_lidar_leave = true;
						println("leave car: lidar leave! start_dist %lld", leave_start_dist);
					}
				}else{
					// 雷达判断离车后再继续移动20cm
					if (xp_odom_get_distance() - leave_start_dist > 200){
						robot_stop();
						println("leave car: odom leave! start %lld now %lld pose[%d, %d]", leave_start_dist, xp_odom_get_distance(),x,y);
						judge_leave_car = true;
					}
				}
			}

			comfirm_leave_cnt = 0;
			comfirm_not_leave_cnt = 0;
		}
		else{														//检测离车后，原地停止确认是否真的离车
			if((distance > ROBOT_CAR_SEARCH_VALUE) && (distance != 255)){
				robot_stop();
				comfirm_not_leave_cnt = 0;
				comfirm_leave_cnt++;
				if((comfirm_leave_cnt > 5) && (now_time - start_time > 20000)){
					isLeaveCar = 1;
					println("comfirm leave car, set leave car flag");
					return 1;
				}
			}
			else{
				comfirm_leave_cnt = 0;
				comfirm_not_leave_cnt++;
				if((comfirm_not_leave_cnt > 5) && (xp_joint_over_range_check() == 0)){
					xp_wheel_set_wheel_speed(-0.08, -0.08);
				}
			}
		}
		aos_msleep(100);
	}
	
	return 0;
}
/**
 * @brief 离开P_A时不需要分段降速
 * 
 * @return int32_t 0表示成功，非0则表示失败。
 */
int xp_robot_leave_car_A(void)
{
	int32_t ret;
	uint64_t start_dist = 0;
	uint64_t diff_dist = 0;
	isLeaveCar = 0;
	ret = xp_wheel_set_wheel_speed(0.15, 0.15);
	if(0 != ret){
		robot_stop();
		return -1;
	}
	start_dist = xp_odom_get_distance();
	aos_msleep(100);
	
	while (1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		diff_dist = xp_odom_get_distance() - start_dist;
		
		if (diff_dist > Leave_car_dist){
			robot_stop();
			isLeaveCar = 1;
			println("leave_car: start_dist %lld cur_dist %lld, diff_dist %lld", start_dist, xp_odom_get_distance(), diff_dist);
			return 1;
		}
		
		aos_msleep(100);
	}
	
	return 0;	
}
/**
 * @brief 离开P_C时采用分段降速，目的是能刷洗到SUV的后挡风玻璃
 * 
 * @return 
 */
int xp_robot_leave_car_C(void)
{
	uint64_t start_dist = 0;
	uint64_t diff_dist = 0;
	int ret = -1;
	u8 err_cnt = 0;

	isLeaveCar = 0;
	start_dist = xp_odom_get_distance();
	aos_msleep(100);

#if xp_config_disabled(WASH_ROOF_RECTANGULAR)
	Leave_car_dist = 700;
#endif
	
	while (1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		diff_dist = xp_odom_get_distance() - start_dist;
		
		if(diff_dist > Leave_car_dist){
			ret = xp_joint_get_efence_state();
			if(ret == 0){							//未触发电子围栏的情况若无法停止，则报异常
				ret = robot_stop();
				if(0 != ret){
					err_cnt++;
					if(err_cnt > 10){
						println("robot stop error! ret %d", ret);
						xp_upper_navi_state_set(1);
						return -1;
					}
					continue;
				}
			}

			isLeaveCar = 1;
			println("leave_car: start_dist %lld cur_dist %lld, diff_dist %lld", start_dist, xp_odom_get_distance(), diff_dist);
			return 1;
		}
		else if(diff_dist > Leave_car_dist/2){
			//超过1/2行程后提高到高速
			xp_wheel_set_wheel_speed(0.15, 0.15);
		}else{
			xp_wheel_set_wheel_speed(0.03, 0.03);
		}

		ret = xp_joint_get_efence_state();
		if((ret != 0) && (diff_dist > 500)){
			isLeaveCar = 1;
			println("leave_car thouch efence force leave fnish: diff_dist %lld",diff_dist);
			return 1;
		}
		
		aos_msleep(100);
	}
	
	return 0;
}
int xp_car_leave_state(void)
{
	return isLeaveCar;
}
/**
 * @brief 机器人停止
 * 
 * @return int32_t 0表示成功，非0则表示失败。
 */
int32_t robot_stop()
{
	int32_t ret = 0;
	
	ret = xp_wheel_set_wheel_speed(0.0, 0.0);
	if(0 != ret){
		println("robot_stop: set_wheel_speed failed! ret %d", ret);
		return -1;
	}else{
		return 0;
	}
}
/**
 * @brief 机器人前进
 * 
 * @return int32_t 0表示成功，非0则表示失败。
 */
int32_t robot_front(float line_speed)
{
	int32_t ret = 0;

	ret = xp_wheel_set_wheel_speed(line_speed, line_speed);
	if(0 != ret){
		println("xp_wheel_set_wheel_speed failed! ret %d", ret);
		return -1;
	}
	else{
		return 0;
	}
}
/**
 * @brief 机器人后退
 * 
 * @return int32_t 0表示成功，非0则表示失败。
 */
int32_t robot_back(float line_speed)
{
	int32_t ret = 0;
	
	ret = xp_wheel_set_wheel_speed(-line_speed, -line_speed);
	if(0 != ret){
		return -1;
	}
	else{
		return 0;
	}
}
/**
 * @brief 返回摆正完成标志位
 *  
 * @return int32_t 0表示摆正完成，非0则表示还未完成。
 */
int32_t xp_is_robot_straightening_end()
{
	return lidar_navi.is_robot_straightening_end;
}
/**
 * @brief 机器人摆正完成标志位复位
 * 
 */
void xp_reset_robot_straightening_end()
{
	lidar_navi.is_robot_straightening_end = 0;
}

void set_robot_straightening_side(int32_t side)
{
	robot_straightening_side = side;
}

void xp_robot_right_angle_signal()
{
	aos_sem_signal(&robot_straightening_sem);
}
/**
 * @brief 机器人摆正
 * @param side，机器人当前所在边
 * @return int32_t 0表示成功，非0则表示失败。
 */
int32_t robot_straightening(int8_t side)
{
	int32_t ret = 0;
	float robot_angle = 0.0;
	uint16_t angle_err_cnt = 0;
#if 0
	println("1st: robot back...");
	ret = robot_back(0.1);
	if(0 != ret){
		println("robot back error! ret %d", ret);
		xp_upper_navi_state_set(1);
		return -1;
	}
	aos_msleep(3000);
	println("2nd: robot stop...");
	robot_stop();
	aos_msleep(2000);
	println("3rd: robot right...");
#endif
	ret = robot_right();
	if(0 != ret){
		println("robot right error! ret %d", ret);
		xp_upper_navi_state_set(1);
		return -1;
	}
	
	robot_angle = get_robot_angle();
	if(side >= 0 && side <= 3){
		while(robot_angle > (side * 90 + 4))
		{
			if(get_emergency_status_flag() == true){
				println("====emergency status, exit navi process");
				return -10;
			}

			robot_angle = get_robot_angle();
			if(robot_angle > 360){
				println("robot_straightening: robot_angle(%f) get failed!", robot_angle);
				angle_err_cnt++;
				if(angle_err_cnt > 10){
					robot_stop();
					return -3;
				}
			}else{
				angle_err_cnt = 0;
			}
			aos_msleep(50);
		}
	}else{
		println("side %d is error!!!", side);
		robot_stop();
		return -2;
	}

	println("4th: robot stop...");
	lidar_navi.is_robot_straightening_end = 1;
	ret = robot_stop();
	if(0 != ret){
		println("robot stop error! ret %d", ret);
		xp_upper_navi_state_set(1);
		return -1;
	}
	
	return 0;
}

static void xp_robot_straightening_run(void *param)
{
	while(1){
	   aos_sem_wait(&robot_straightening_sem, AOS_WAIT_FOREVER);
	   println("robot_side %d straightening......", robot_straightening_side);
	   robot_straightening(robot_straightening_side);
   }
}
#if 0
int32_t xp_robot_right_turn_angle(float angle)
{
	int32_t ret = 0;
	float cur_car_angle = 0.0;
	float last_car_angle = 0.0;
	if(angle < 0 || angle > 45){
		return -1;
	}
	last_car_angle = get_gyro_angle();
	
	ret = xp_wheel_set_wheel_speed(-0.025, 0.025);
	if(0 != ret){
		robot_stop();
		println("robot left error! ret %d", ret);
		xp_upper_navi_state_set(1);
		return -2;
	}
	cur_car_angle = get_gyro_angle();
	while(cur_car_angle - last_car_angle < angle)
	{
		cur_car_angle = get_gyro_angle();
		aos_msleep(20);
	}
	
	ret = robot_stop();
	if(0 != ret){
		println("robot stop error! ret %d", ret);
		xp_upper_navi_state_set(1);
		return -3;
	}
	return 0;
}
#endif
/**
 * @brief 机器人原地右拐
 * 
 * @return int32_t 0表示成功，非0则表示失败。
 */
int32_t robot_right()
{
	int32_t ret = 0;
	
	ret = xp_wheel_set_wheel_speed(0.05, -0.05);
	if(0 != ret){
		return -1;
	}
	else{
		return 0;
	}
}
/**
 * @brief 机器人原地左拐
 * 
 * @return int32_t 0表示成功，非0则表示失败。
 */
int32_t robot_left()
{
	int32_t ret = 0;
	
	ret = xp_wheel_set_wheel_speed(-0.1, 0.1);//18度/s
	if(0 != ret){
		return -1;
	}
	else{
		return 0;
	}
}
#if 0
int32_t GetAngleIncrement(int32_t angle1, int32_t angle2)
{
	int32_t temp;
	if (angle1 < angle2){
		temp = angle2;
		angle2 = angle1;
		angle1 = temp;
	}
	if ((temp = angle1 - angle2) > 180){
		temp = 360 - angle1 + angle2;
	}
	return temp;
}
#endif
/**
 * @brief 机器人矩形拐弯
 * 
 * @return int32_t 0表示机器人矩形拐弯正常运行，非0则表示矩形拐弯失败。
 */
#if 1
int32_t xp_robot_rectangular_turn()
{
	int32_t ret = -1;
	int32_t lidar_dist = 0;
	uint8_t wait_cnt = 0;
	uint8_t close_cnt = 0;
	uint8_t too_close_cnt = 0;
	bool negative_roation_flag = false;
	
	ret = xp_local_turn(90000);
	if(ret < 0){
		println("xp_local_turn failed! ret %d", ret);
		return ret;
	}
	else if(1 == ret){
		//如果是提前检测到车停止转弯，则无需直行，直接开始绕边
		println("xp_local_turn early stop! NO need robot_front, directly robot_going!");
	}

	//旋转完毕后，确认转完后的雷达距离，若雷达距离过近进行角度调整
	wait_cnt = 0;
	while(wait_cnt < 10){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		lidar_dist = xp_navi_lidar_get();

		if(lidar_dist <= 75)
		{
			too_close_cnt++;
			if(too_close_cnt > 3)
			{
				negative_roation_flag = true;
				println("too close, need negative rotation !!!");
				break;
			}
		}

		wait_cnt++;
		aos_msleep(100);
	}

	if(true == negative_roation_flag){
		//直角转弯距离过近时，右转45度
		ret = xp_local_turn(-45000);
		if(ret < 0){
			println("xp_local_turn failed! ret %d", ret);
		}
	}
	else{
		wait_cnt = 0;
		//正常转弯结束，直行一段距离，避免绕边时找不到车
		while(wait_cnt < 4){
			if(get_emergency_status_flag() == true){
				println("====emergency status, exit navi process");
				return -10;
			}

			lidar_dist = xp_navi_lidar_get();

			if(lidar_dist < 85){
				//直行过程中距离过近，提前退出，避免撞到车
				close_cnt++;
				if(close_cnt > 3)
				{
					println("close, dont go straight");
					break;
				}
				else
				{
					aos_msleep(100);
				}
			}else{
				close_cnt = 0;
				robot_going_line(0.15);
				aos_msleep(1000);
				wait_cnt++;
			}
		}
	}
	robot_stop();

	lidar_navi.robot_turning_end = 1;
	return 0;
}
#else
int32_t xp_robot_rectangular_turn()
{
	int32_t ret = 0;
	int16_t distance = 0;
	int32_t startAngle = (int32_t)(get_gyro_angle() + 0.5);
	int32_t curAngle = (int32_t)(get_gyro_angle() + 0.5);
	int32_t incAngle = curAngle - startAngle;
	bool b_front = false;
	bool b_stop = false;
	bool is_low_speed = false;
	bool is_agv_stop = false;
	uint8_t step = 0;
	uint64_t last_time;
	uint8_t count = 0;
	uint8_t tryTimes = 5;
	
	while (0 != (ret = robot_left()) && --tryTimes){
		aos_msleep(500);
	}
	if (0 == tryTimes)
	{
		println("robot_left error! ret %d", ret);
		return -1;
	}
	aos_msleep(3000);

	//左拐90°
	while (1)
	{
#if 1
		distance = xp_navi_lidar_get();
		curAngle = (int32_t)(get_gyro_angle() + 0.5);
		incAngle = GetAngleIncrement(startAngle, curAngle);
		//chenwenbo 12-03  ROBOT_TURN_LIDAR_VALUE - 15	增加角度判断   (distance < (ROBOT_TURN_LIDAR_VALUE - 20) && 0 != distance) || 
		if(incAngle > 60)
		{
			printf(">>>>>rectangular_turn inc_angle %d start_angle %d cur_angle %d, dis:%d\r\n", 
				incAngle, startAngle, curAngle, distance);
			robot_stop();
			if(false == b_front){
				ret = robot_front();
				if(ret != 0){
					continue;
				}else{
					b_front = true;
					aos_msleep(4000);
				}
			}
			if(false == b_stop){
				ret = robot_stop();
				if(ret != 0){
					continue;
				}else{
					b_stop = true;
				}
			}
			lidar_navi.robot_turning_end = 1;
			return 0;
		}
#else

		distance = xp_navi_lidar_get();
		curAngle = (int32_t)(get_gyro_angle() + 0.5);
		incAngle = GetAngleIncrement(startAngle, curAngle);

		switch (step)
		{
		case 0:
			if (incAngle > Angle_1)
			{ //在角度达到此参数时开始启用雷达作为转弯结束的判定
				step = 1;
				count = 0;
			}
			break;

		case 1:
			if (incAngle > Angle_2 && !is_agv_stop)
			{ //在角度达到此参数时开始停止转弯（考虑到减速为0的时间，最终将转到90度）
				ret = robot_stop();
				if(ret != 0){
					continue;
				}
				is_agv_stop = true;
				step = 2;
				count = 0;
			}

//			if (distance < Pid_allow_range + 15 && !is_agv_stop)
//			{ //雷达距离在进入PID可正常调节范围内停车
//				robot_stop();
//				is_agv_stop = true;
//				step = 2;
//				count = 0;
//			}
			last_time = aos_now_ms();
			break;

		case 2:
			if (is_agv_stop && (aos_now_ms() - last_time > 1500)) //这里等几秒彻底停下来
			{
				if (distance > Pid_allow_range)
				{
					step = 3; //矩形转弯第一阶段停止后距离车尾仍过远
					count = 0;
				}
				else
				{
					step = 5; //矩形转弯第一阶段停止后与车尾距离合适
					count = 0;
				}
			}
			break;

		case 3:
			ret = xp_wheel_set_wheel_speed(-0.05, 0.05); //离车尾过远时向内转一定角度再直行直到超时或者距离检测合适
			if (0 != ret)
			{
				continue;
			}
			aos_msleep(1000);
			ret = xp_wheel_set_wheel_speed(0.08, 0.08);
			if (0 != ret)
			{
				continue;
			}
			step = 4;
			count = 0;
			last_time = aos_now_ms();
			break;
		case 4:
			if (aos_now_ms() - last_time < 20000) //允许时间段内直行直到与车尾距离合适
			{
				if (distance < Pid_allow_range)
				{
					if (count++ >= 3){
						robot_stop();
						is_agv_stop = true;
						aos_msleep(500);
						ret = xp_wheel_set_wheel_speed(0.05, -0.05); //摆正
						if (0 != ret){
							continue;
						}
						aos_msleep(1000);
						robot_stop();
						step = 7;
					}
				}else{
					count = 0;
				}
			}
			else
			{
				//错误  直行超时 仍未达到合适距离
				ret = robot_stop();
				print("Over Time Not Find Car!!!\r\n");
				return -1;
			}
			break;

		case 5:
			ret = xp_wheel_set_wheel_speed(0.10, 0.10);
			if (0 != ret)
			{
				continue;
			}
			step = 6;
			count = 0;
			last_time = aos_now_ms();
			break;

		case 6:
		#if 0
			if (aos_now_ms() - last_time > 5000) //允许时间段内直行直到与车尾距离合适
			{
				if (distance < Pid_allow_range)
				{
					if (count++ >= 3){
						ret = robot_stop();
						if (0 != ret)
						{
							return -1;
						}
						is_agv_stop = true;
						step = 7;
					}
				}else{
					count = 0;
				}
			}
			else
			{
				//错误  直行超时 仍未达到合适距离
				return -1;
			}
		#else
			if (aos_now_ms() - last_time > 3500) //行走固定时间
			{
				
				robot_stop();
				is_agv_stop = true;
				step = 7;
			}
			if (distance < 100){
				if (count++ > 4){
					robot_stop();
					is_agv_stop = true;
					step = 7;
				}
			}else{
				count = 0;
			}
		#endif
		break;
		
		case 7:
			lidar_navi.robot_turning_end = 1;
			return 0;
			break;
		}

#endif
		aos_msleep(100);
	}
}
#endif

/**
 * @brief 判断是否是矩形拐弯
 * 
 * @param type type为0，2为矩形拐弯，在洗车顶时用到。
 * @return int32_t 1表示是矩形拐弯类型，0则表示不是。
 */
static int32_t is_rectangular_turn(int8_t type)
{
	if(0 == type || 2 == type){
		return 1;
	}else{
		return 0;
	}
}

/**
 * @brief 判断当前机器角度是否在目标边的角度范围内
 * 
 * @param robot_angle 机器当前角度
 * @param target_side 目标边
 * @param angle_range 角度范围
 * @return true表示在范围内，false表示不在范围内。
 */
bool is_angle_in_side_range(float robot_angle, int8_t target_side, float angle_range)
{
	//println("target_side %d robot_angle %.1f angle_range %.1f", target_side, robot_angle, angle_range);

	if(robot_angle > 360){
		//角度不对时默认返回false
		return false;
	}

	target_side %= 4;
	if(0 == target_side){
		if(robot_angle > (360 - angle_range) || robot_angle < angle_range){
			return true;
		}else{
			return false;
		}
	}else{
		if(robot_angle > (target_side*90 - angle_range) && robot_angle < (target_side*90 + angle_range)){
			return true;
		}else{
			return false;
		}
	}

	return false;
}

/**
 * @brief 机器人pid拐弯
 * @param side 机器人所在边，在洗车顶与回待机位时用到
 * @return int32_t 0表示机器人拐弯正常运行，非0则表示拐弯失败。
 */
int32_t xp_robot_pid_turn(int8_t side)
{
	int32_t ret = 0;
	int16_t dist = 0;
	float robot_angle = 0.0;
	long long start_time = 0, cur_time =0;
	int err_cnt = 0;
	int8_t next_side = (side + 1) % 4;
	
	start_time = aos_now_ms();
	cur_time = aos_now_ms();
	
	while(1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		if(cur_time - start_time > ROBOT_PID_TURN_TIMEOUT){
			robot_stop();
			println("robot_pid_turn timeout!");
			return -1;
		}
		
		robot_angle = get_robot_angle();
		if(robot_angle > 360){
			robot_stop();
			println("robot_pid_turn: robot_angle(%f) get failed!", robot_angle);
			continue;
		}
		dist = xp_navi_lidar_get();

		//PID弧形拐弯到与车基本平行，通过角度和距离判断
		if(is_angle_in_side_range(robot_angle, next_side, pid_turn_angle) && (dist != 0 && dist < ROBOT_CAR_SEARCH_VALUE)){
			ret = robot_stop();
			if(0 != ret){
				err_cnt++;
				if(err_cnt > 10){
					println("robot_stop error! ret %d", ret);
					xp_upper_navi_state_set(1);
					return -1;
				}
			}else{
				err_cnt = 0;
			}
			lidar_navi.robot_turning_end = 1;
			println("robot_pid_turn end, robot_angle %.3f", robot_angle);
			return 0;
		}else{
			ret = robot_going();
			if(0 != ret){
				err_cnt++;
				if(err_cnt > 10){
					println("robot_going error! ret %d", ret);
					robot_stop();
					xp_upper_navi_state_set(1);
					return -1;
				}
			}else{
				err_cnt = 0;
			}
		}
		
		aos_msleep(100);
		cur_time = aos_now_ms();
	}
}

int32_t xp_back_standy_turn(uint32_t back_dist)
{
	uint64_t start_dist = xp_odom_get_distance();
	int ret, err_cnt = 0;
	
	print("xp_back_standy Start\r\n");
	while (xp_odom_get_distance() - start_dist < back_dist){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		print("start_dist:%lld curr_dist:%lld\r\n", start_dist, xp_odom_get_distance());
		ret = robot_going();
		if(0 != ret){
			err_cnt++;
			if(err_cnt > 10){
				println("robot_going error! ret %d", ret);
				robot_stop();
				xp_upper_navi_state_set(1);
				return -1;
			}
		}else{
			err_cnt = 0;
		}
		aos_msleep(100);
	}
	ret = robot_stop();
	if (0 != ret){
		xp_upper_navi_state_set(1);
		return -1;
	}
	lidar_navi.robot_turning_end = 1;
	print("xp_back_standy_turn End\r\n");
}

/**
 * @brief 机器人拐弯
 * 
 * @param type 机器人拐弯类型，0，2是矩形拐弯，1，3为pid拐弯与小车平行
 */
void xp_robot_turning(int8_t type)
{
	int32_t ret = 0;
	
	xp_reset_robot_turning();
	
	if(is_rectangular_turn(type)){
		ret = xp_robot_rectangular_turn();
	}else{
		if(98 == type){
			ret = xp_back_standy_turn(1000);
		}else if(99 == type){
			ret = xp_back_standy_turn(2000);
		}else{
			ret = xp_robot_pid_turn(type);
		}
	}
	
	if(0 != ret){
		println("robot turn error! type %d, ret %d", type, ret);
		xp_upper_navi_state_set(1);
	}
}
/**
 * @brief 机器人拐弯复位
 * 
 */
void xp_reset_robot_turning()
{
	lidar_navi.robot_turning_end = 0;
}
/**
 * @brief 机器人拐弯是否结束
 * 
 * @return int32_t 0表示机器人拐弯结束，非0则表示拐弯没有结束。
 */
int32_t xp_is_robot_turning_end()
{
	return lidar_navi.robot_turning_end;
}

/**
 * @brief 机器人旋转到与某条边对齐
 * 
 * @param side 原地旋转的目标边
 */
int32_t xp_robot_turn_to_side(int8_t side)
{
	int32_t x, y;
	float angle = 0;
	uint8_t wait_cnt = 0;

	//计算目标角度
	angle = fmod(side * 90 + robot_angle_zero_offset, 360);

	xp_joint_turn_to_angle(angle);
	while( !xp_joint_is_move_complete() ){
		if(get_emergency_status_flag() == true){
			println("========emergency status, exit joint move process");
			return -10;
		}

		aos_msleep(100);
		wait_cnt++;
		if(wait_cnt > 300){
			/* 此处应该还要再加个joint_move_exit() */
			println("xp_robot_turn_to_side: timeout!");
			return -1;
		}
	}

	//获取旋转后的当前位姿
	xp_joint_get_robot_pose(&x, &y, &angle);
	println("xp_robot_turn_to_side: complete, cur_pose [%d, %d, %.1f]", x, y, angle);

	return 0;
}

/**
 * @brief 设置pid绕行的基准速度
 * 
 * @param val 基准速度值
 */
void set_robot_base_speed(float val)
{
	lidar_navi.robot_base_speed = val;
}

/**
 * @brief 设置pid绕行的补偿值，由毛刷压力检测引入
 */
void set_robot_pid_offset(float val)
{
	lidar_navi.robot_pid_offset = val;
}

static bool is_ultrasound_detected()
{
	if(1 == xp_agv_io_read(SAFTY_SIDE_1) || 1 == xp_agv_io_read(SAFTY_SIDE_2)){
		
		aos_msleep(100);
		
		if(1 == xp_agv_io_read(SAFTY_SIDE_1) || 1 == xp_agv_io_read(SAFTY_SIDE_2)){
			println("us detected! %d %d", xp_agv_io_read(SAFTY_SIDE_1), xp_agv_io_read(SAFTY_SIDE_2));
			return 1;
		}else{
			return 0;
		}
		
	}else{
		return 0;
	}
}
/**
 * @brief 机器人pid行走
 * 
 * @return int32_t 0表示机器人pid行走正常，非0则表示pid行走异常。
 */
static int32_t robot_going()
{
	int32_t ret = 0;
	int16_t pid_in = 0;//PID输入
	float pid_out = 0;//PID输出
	int16_t lidar_dist = 0;
	int16_t obstacle_dist = 0;
	static u8 loop_cnt = 0;
	static bool b_need_right_recover = false;//需右转恢复标志
	static bool b_need_wait_recover = false;//等待雷达恢复标志
	static float speed_record=0;
	float speed_l = 0;
	float speed_r = 0;
	float speed_max = 0;
	float w_rad, w_deg;//机器角速度
	float turn_radius;//机器拐弯半径
	SAFTY_STATE_E safe_state = SAFTY_state_lidar_abnormal;

	loop_cnt++;
	lidar_dist = xp_navi_lidar_get();
	
	//存在门没吸合的情况，所以这段距离需要屏蔽掉
	/*if(0 != lidar_dist && lidar_dist < ROBOT_WIDTH_VALUE){
		return 0;
	}*/

	//如果是距离车体过近导致的遇障，则尝试右转恢复
	obstacle_dist = xp_navi_lidar_get3();
	if(true == b_need_right_recover){
		if(obstacle_dist < ROBOT_SAFE_DISTANCE_VALUE+10){
			robot_right();
		}else{
			b_need_right_recover = false;
		}
		return 0;
	}

	safe_state = xp_safty_check(LIDAR_roi_obstacle, ROBOT_WIDTH_VALUE);
	if(SAFTY_state_lidar_abnormal == safe_state){
		//雷达距离获取不到时不进行pid调节，避免机器大幅往里拐。
		//并且恢复后，距离需小于极限值才继续绕边，因刚恢复时距离一直会输出极限值，容易导致机器往里拐
		b_need_wait_recover = true;
		robot_stop();
		return 0;
	}else if(SAFTY_state_too_near == safe_state){
		//雷达距离过近时不进行pid调节，尝试右转恢复
		b_need_right_recover = true;
		robot_stop();
		return 0;
	}else if(SAFTY_state_obstacle == safe_state){
		//雷达距离遇障时不进行pid调节，原地停止
		robot_stop();
		return 0;
	}else{
		//雷达距离正常，如果之前有出现过失联，恢复后，距离需小于极限值才继续绕边，因刚恢复时距离一直会输出极限值，容易导致机器往里拐
		if(true == b_need_wait_recover){
			if(lidar_dist >= LIDAR_DIST_MAX_VALUE){
				println("====wait v_lidar recover!");
				return 0;
			}else{
				b_need_wait_recover = false;
			}
		}
	}

	if(xp_wheel_get_wheel_locker()){
		println("wheel locker! robot stop!");
		ret = robot_stop();
		if(0 != ret){
			println("robot_stop error! ret %d", ret);
			return -1;
		}
			
		return 0;
	}
	
	//雷达扫描不到车子的情况，给其一点弧度，让雷达扫到来转弯。
	if(lidar_dist >= LIDAR_DIST_MAX_VALUE){
		//拐弯半径r=WHEEL_dist*(v2+v1)/(2*(v2-v1))=644*(1.6*0.15)/(2*0.4*0.15)=1288mm
		ret = xp_wheel_set_wheel_speed(lidar_navi.robot_base_speed * 3 / 5, lidar_navi.robot_base_speed);
		if(0 != ret){
			println("xp_wheel_set_wheel_speed error! ret %d", ret);
			return -1;
		}
		return 0;
	}
	
	pid_in = lidar_dist;
	lidar_navi.pid.last_error = 0;
	lidar_navi.pid.prev_error = 0;
	lidar_navi.pid.Kp = lidar_navi.robot_base_speed / 75.0f;
	if (lidar_navi.pid.Kp < 0.0001f){
		lidar_navi.pid.Kp = 0.0001f;
	}
	pid_out = PIDx_calc(&(lidar_navi.pid), pid_in);
	if(pid_out > PID_INCREMENTAL_POS_THRESHOLD){
		println("robot_going: pid_out %f > threshold\n", pid_out);
		pid_out = PID_INCREMENTAL_POS_THRESHOLD;
	}
	if(pid_out < PID_INCREMENTAL_NEG_THRESHOLD){
		println("robot_going: pid_out %f < threshold\n", pid_out);
		pid_out = PID_INCREMENTAL_NEG_THRESHOLD;
	}
	pid_out = pid_out * f_K + last_pid_out * (1 - f_K);
	last_pid_out = pid_out;
	pid_out = pid_out + lidar_navi.robot_pid_offset;
	speed_l = lidar_navi.robot_base_speed + pid_out;
	speed_r = lidar_navi.robot_base_speed - pid_out;
	speed_max = lidar_navi.robot_speed_max;
#if 0
	// if (fabs(pid_out) > PID_INCREMENTAL_POS_THRESHOLD * 0.6){
	// 	//拐弯初期，最小拐弯半径r=WHEEL_dist*(v2+v1)/(2*(v2-v1))=WHEEL_dist*(2*vbase)/(2*2*0.07)=644*0.3/0.24=690mm
	// 	//println("robot_going: pid_out %f > 0.6, l %f r %f max %f\n", pid_out, speed_l, speed_r, speed_max);
	// }
	// else 
	if (fabs(pid_out) > PID_INCREMENTAL_POS_THRESHOLD * 0.4){
		//拐弯后期降速，防止拐过，最小拐弯半径r=WHEEL_dist*(2*vbase)/(2*2*0.06)=644*0.3/0.24=805mm
		speed_l *= 0.7;
		speed_r *= 0.7;
		speed_max *= 0.7;
		//println("robot_going: pid_out %f > 0.4, l %f r %f max %f\n", pid_out, speed_l, speed_r, speed_max);
	}
	else if (fabs(pid_out) > PID_INCREMENTAL_POS_THRESHOLD * 0.2){
		//直线段小幅调整，最小拐弯半径r=WHEEL_dist*(2*vbase)/(2*2*0.4*0.06)=644*0.3/0.096=2012.5mm
		speed_l *= 0.8;
		speed_r *= 0.8;
		speed_max *= 0.8;
		//println("robot_going: pid_out %f > 0.2, l %f r %f max %f\n", pid_out, speed_l, speed_r, speed_max);
	}else{
		//直线段微调，最小拐弯半径r=WHEEL_dist*(2*vbase)/(2*2*0.2*0.07)=644*0.3/(2*2*0.014)=3450mm
	}
#endif
	if(speed_l > speed_max)
	{
		speed_l = speed_max;
	}
	if(speed_l < -speed_max)
	{
		speed_l = -speed_max;
	}
	if(speed_r > speed_max)
	{
		speed_r = speed_max;
	}
	if(speed_r < -speed_max)
	{
		speed_r = -speed_max;
	}

	//根据左右轮命令速度计算机器角速度和拐弯半径
	w_rad = (speed_r - speed_l) * 1000 / WHEEL_dist;
	w_deg = w_rad * RAD2DEG;
	turn_radius = WHEEL_dist * (speed_r + speed_l) / (2 * (speed_r - speed_l));
	if(loop_cnt % 10 == 0){
		println("robot_going: pid_in %d set_point %.1f, pid_out %.3f offset %.3f, l %.3f r %.3f, w %.3f %.3f r %.3f", 
			pid_in, lidar_navi.pid.setpoint, pid_out, lidar_navi.robot_pid_offset, speed_l, speed_r, w_rad, w_deg, turn_radius);
	}
	
	ret = xp_wheel_set_wheel_speed(speed_l,speed_r);
	if(0 != ret){
		println("xp_wheel_set_wheel_speed, ret is %d\n", ret);
		return -1;
	}
   
	return 0;
}

/**
 * @brief 机器人直线行走
 * 
 * @return int32_t 0表示机器人直线行走正常，非0则表示直线行走异常。
 */
static int32_t robot_going_line(float line_speed)
{
	int32_t ret = 0;
	int16_t lidar_dist = 0;
	SAFTY_STATE_E safe_state = SAFTY_state_lidar_abnormal;
	static bool b_need_wait_recover = false;//等待雷达恢复标志

	safe_state = xp_safty_check(LIDAR_roi_obstacle, ROBOT_WIDTH_VALUE);
	if(SAFTY_state_lidar_abnormal == safe_state){
		//雷达距离获取不到时暂停移动
		//并且恢复后，距离需小于极限值才继续移动，因刚恢复时距离一直会输出极限值
		b_need_wait_recover = true;
		robot_stop();
		return 0;
	}else if(SAFTY_state_obstacle == safe_state){
		//雷达距离遇障时原地停止
		robot_stop();
		return 0;
	}else{
		//雷达距离正常，如果之前有出现过失联，恢复后，距离需小于极限值才继续移动，因刚恢复时距离一直会输出极限值
		if(true == b_need_wait_recover){
			lidar_dist = xp_linux_lidar_dis_get();
			if(lidar_dist >= LIDAR_DIST_MAX_VALUE){
				println("====wait v_lidar recover!");
				return 0;
			}else{
				b_need_wait_recover = false;
			}
		}
	}

	if(xp_wheel_get_wheel_locker()){
		println("wheel locker! robot stop!");
		ret = robot_stop();
		if(0 != ret){
			println("robot_stop error! ret %d", ret);
			return -1;
		}
			
		return 0;
	}
	
	ret = robot_front(line_speed);
	if(0 != ret){
		println("robot_front failed! ret %d\n", ret);
		return -1;
	}
   
	return 0;
}

/**
 * @brief 机器人以joint_move方式直线行走
 * 
 * @return int32_t 0表示机器人直线行走正常，非0则表示直线行走异常。
 */
static int32_t robot_going_joint_move(bool b_new_point, Type_Vector3_Def tar_point)
{
	int32_t ret = 0;
	int16_t lidar_dist = 0;
	static u8 obstacle_cnt = 0;
	static u8 lidar_err_cnt = 0;
	static bool b_tar_point_set_success = false;
	static bool lidar_err_flag = false;

#if 0
	lidar_dist = xp_navi_lidar_get();

	if(!xp_navi_lidar_status()){
		lidar_err_cnt++;
		if(lidar_err_cnt > 3){
			//超过300ms获取不到雷达距离则暂停运行
			println("v_lidar is offline!");
			xp_upper_v_lidar_state_set(255);
			robot_stop();
			lidar_err_flag = true;
		}
		//雷达距离获取不到时不进行后续判断
		return 0;
	}else{
		if(true == lidar_err_flag){
			if(lidar_dist < LIDAR_DIST_MAX_VALUE){
				//雷达恢复后，距离需小于极限值才继续绕边，因刚恢复时距离一直会输出极限值，容易导致机器往里拐
				lidar_err_flag = false;
				xp_upper_v_lidar_state_set(0);
				lidar_err_cnt = 0;
				println("====v_lidar is recover!");
			}
			else{
				println("====wait v_lidar recover!");
				return 0;
			}
		}else{
			xp_upper_v_lidar_state_set(0);
			lidar_err_cnt = 0;
		}
	}

	//判断遇障
	if(0 != lidar_dist && lidar_dist < ROBOT_WIDTH_VALUE){
		obstacle_cnt++;
		if(obstacle_cnt > 3){
			robot_stop();
			xp_upper_stoped_set(1);
			println("robot_going_line: lidar_dist %d < %d, robot_stop!", lidar_dist, ROBOT_WIDTH_VALUE);
		}
		return -2;
	}else{
		obstacle_cnt = 0;
		xp_upper_stoped_set(0);
	}
#endif
	
	//之前设置成功的目标点不需要每次都设
	if(true == b_new_point || false == b_tar_point_set_success){
		ret = xp_joint_move_to_point(tar_point.x, tar_point.y, tar_point.z);
		if(0 != ret){
			b_tar_point_set_success = false;
			println("xp_joint_move_to_point [%.1f,%.1f,%.1f] failed! ret %d\n", tar_point.x, tar_point.y, tar_point.z, ret);
			return -3;
		}else{
			b_tar_point_set_success = true;
			println("xp_joint_move_to_point [%.1f,%.1f,%.1f] success!\n", tar_point.x, tar_point.y, tar_point.z);
		}
	}
   
	return 0;
}

/**
 * @brief 机器人绕车一边是否完成
 * 
 * @return int32_t 1表示绕车一边完成，非0则表示未完成。
 */
int32_t xp_is_robot_round_side_end()
{
	return lidar_navi.robot_round_side_end;
}
/**
 * @brief 机器人绕车一边行走复位
 * 
 */
void xp_reset_robot_round_side()
{
	lidar_navi.mean_square_error_value = 100;
	lidar_navi.mean_square_index = 0;
	lidar_navi.robot_round_side_end = 0;
}
/**
 * @brief 机器人绕车一边行走停止
 * 
 */
void xp_set_robot_round_side_stop()
{
	lidar_navi.robot_round_side_stop = 1;
}

int32_t xp_is_round_side_leave_car()
{
	return lidar_navi.is_robot_round_side_leave_car;
}

void xp_reset_round_side_leave_car()
{
	lidar_navi.is_robot_round_side_leave_car = 0;
}

float get_mean_square_error(int32_t *lidar_distance, int32_t size, int32_t average)
{
	int i;
	float sum2=0.0;
	float variance;    
	float mean_square_error_value = 0.0;
	for(i = 0; i < size; i++){
		sum2+=(lidar_distance[i]-average)*(lidar_distance[i]-average);
	}
	variance= sum2/size;
	mean_square_error_value = sqrt(variance);
	//println("mean_square_error = %f", mean_square_error_value);
	return mean_square_error_value;
}

/**
 * @brief 判断是否是单机模式
 */
bool is_single_robot_mode(void){
	if(0 == another_robot_side && 0 == another_robot_pose.p_x && 0 == another_robot_pose.p_y){
		return true;
	}
	else{
		return false;
	}
}

/**
 * @brief 双机管线架碰撞判断函数
 * 
 * @param side机器人当前行走的是哪条边
 */
bool check_robot_pipeline_collision(u8 side)
{
	ROBOT_POSE cur_pose;
	uint32_t run_dist = 0;//双机间的行走距离，非直线距离
	static uint32_t count = 0;

	count++;
	if(true == is_single_robot_mode()){
		//如果另一个机器的坐标和边都为0，说明当前是单机模式，不需要判断管线架碰撞
		goto END;
	}else if(2 == abs(side - another_robot_side)){
		//如果是对角边，不会相撞
		b_detect_pipe_collision = false;
		return false;
	}else if((side + 1) % 4 == another_robot_side){
		//如果是邻边且本机处于后方，需计算离前方机器的直角距离
		get_robot_pose(&cur_pose);
		run_dist = abs(cur_pose.p_x - another_robot_pose.p_x) + abs(cur_pose.p_y - another_robot_pose.p_y);
		if(false == b_detect_pipe_collision){
			if(run_dist < PIPELINE_COLLISION_DIST_THRESHOLD){
				if(count % 5 == 0){
					println("pipeline_collision: self_pose[%d,%d] another_pose[%d,%d] run_dist %d, detect collision!!!",
						cur_pose.p_x, cur_pose.p_y, another_robot_pose.p_x, another_robot_pose.p_y, run_dist);
				}
				b_detect_pipe_collision = true;
				return true;
			}
		}else{
			//如果已检测到碰撞，则需要超过检测阈值一定距离后才能解除
			if(run_dist > (PIPELINE_COLLISION_DIST_THRESHOLD+500)){
				if(count % 5 == 0){
					println("pipeline_collision: self_pose[%d,%d] another_pose[%d,%d] run_dist %d, detect NO collision!",
						cur_pose.p_x, cur_pose.p_y, another_robot_pose.p_x, another_robot_pose.p_y, run_dist);
				}
				b_detect_pipe_collision = false;
				return false;
			}else{
				if(count % 5 == 0){
					println("pipeline_collision: self_pose[%d,%d] another_pose[%d,%d] run_dist %d, collision Lock!",
						cur_pose.p_x, cur_pose.p_y, another_robot_pose.p_x, another_robot_pose.p_y, run_dist);
				}
				return true;
			}
		}
	}else if(side == another_robot_side){
		//如果处于同边，后方的机器需要计算离前方机器的距离
		get_robot_pose(&cur_pose);
		run_dist = sqrt(pow(cur_pose.p_x - another_robot_pose.p_x, 2) + pow(cur_pose.p_y - another_robot_pose.p_y, 2));
		if(((0 == side) && (cur_pose.p_x < another_robot_pose.p_x))      //0号边后方机器
			|| ((1 == side) && (cur_pose.p_y < another_robot_pose.p_y))  //1号边后方机器
			|| ((2 == side) && (cur_pose.p_x > another_robot_pose.p_x))  //2号边后方机器
			|| ((3 == side) && (cur_pose.p_y > another_robot_pose.p_y))) //3号边后方机器
		{
			if(false == b_detect_pipe_collision){
				if(run_dist < PIPELINE_COLLISION_DIST_THRESHOLD){
					println("pipeline_collision: self_pose[%d,%d] another_pose[%d,%d] run_dist %d, detect collision!!!",
						cur_pose.p_x, cur_pose.p_y, another_robot_pose.p_x, another_robot_pose.p_y, run_dist);
					b_detect_pipe_collision = true;
					return true;
				}
			}else{
				//如果已检测到碰撞，则需要超过检测阈值一定距离后才能解除
				if(run_dist > (PIPELINE_COLLISION_DIST_THRESHOLD+500)){
					println("pipeline_collision: self_pose[%d,%d] another_pose[%d,%d] run_dist %d, detect NO collision!",
						cur_pose.p_x, cur_pose.p_y, another_robot_pose.p_x, another_robot_pose.p_y, run_dist);
					b_detect_pipe_collision = false;
					return false;
				}else{
					if(count % 5 == 0){
						println("pipeline_collision: self_pose[%d,%d] another_pose[%d,%d] run_dist %d, collision Lock!",
							cur_pose.p_x, cur_pose.p_y, another_robot_pose.p_x, another_robot_pose.p_y, run_dist);
					}
					return true;
				}
			}
		}
	}else{
		//其他情况，不会相撞
		b_detect_pipe_collision = false;
		return false;
	}
END:
	b_detect_pipe_collision = false;
	return false;
}

/**
 * @brief 是否完成绕边找平
 * 
 * @param 
 */
bool is_find_car_parallel(void)
{
	uint64_t now_dist = 0;
	static uint64_t last_dist = 0;
	static int lidar_distance[20];
	static int j = 0;

	now_dist = xp_odom_get_distance();

	//小车需要前进一段距离后才进行记录判断，防止原地停止距离一直一样，直接找平了
	if((now_dist - last_dist) > 10){
		//雷达测得的最近20个距离值均方差小于一定值时则认为找平
		if(lidar_navi.mean_square_error_value > 3){
			if(lidar_navi.mean_square_index==20){
				for(j=0;j<19;j++)
				{
					lidar_distance[j] = lidar_distance[j+1];
				}
				
				lidar_distance[lidar_navi.mean_square_index-1] = xp_navi_lidar_get();
				lidar_navi.mean_square_error_value = get_mean_square_error(lidar_distance, 20, lidar_navi.robot_safe_distance);
			}
			else{
				lidar_distance[lidar_navi.mean_square_index] = xp_navi_lidar_get();
				lidar_navi.mean_square_index++;
			}
		}
		else
		{
			println("find car parallel ok");
			return 1;			//找平成功
		}

		last_dist = now_dist;
	}
	return 0;					//未找平成功，返回0值
}


/**
 * @brief 机器人绕车一边行走
 * 
 * @param side机器人绕车行走的是哪条边
          若side/4=1，表示要进行上下刷洗——洗车机
          若side/4=2，表示是直角洗车顶边——洗车机
          若side/4=3，表示绕边最小运行距离要特殊处理——吹风机找平车辆后判断绕边结束需要
          若side/4=4，表示该边要提前在某个位置绕边结束——洗车机空跑到离车检测位置需要
 */
void xp_robot_round_side(int8_t side, uint32_t timeout)
{
	int32_t ret = 0;
	long long start_time = 0, cur_time = 0;
	uint64_t start_dist = 0;
	uint64_t diff_dist = 0;
	uint64_t leave_car_start_dist = 0;
	uint32_t leave_car_diff_dist = (ROBOT_LENGTH_VALUE-25)*10;
	uint32_t dist_threshold = 0;
	int i = 0;
	int leave_detect_count = 0, loop_cnt = 0;
	int err_cnt = 0, wait_cnt = 0;
	int lidar_dist2 = 0;
	bool turn_flag = false;
	bool b_soft_reset_complete = false;
	bool b_no_need_soft_reset = false;
	bool b_car_y_identify = false;
	uint8_t move_mode = 0;//0-stop, 1-going, 2-front
	int32_t p_x = 0, p_y = 0;
	int32_t start_x, start_y;
	float start_angle;
	int32_t leave_x, leave_y;
	int32_t now_x;
	float leave_angle;
	int32_t leave_dist;
	Type_Vector3_Def first_tar_point;//找平后初始规划的目标点
	Type_Vector3_Def final_tar_point;//离车后最终规划的目标点
	bool b_need_plan_first_tar_point = true;//是否需要规划初始目标点
	bool b_first_point_set_success = false;
	bool b_final_point_set_success = false;
	
	xp_linux_lidar_blow();				//开始绕边前先对雷达吹气，消除水珠

	//移动前先复位绕边行走结束标志、离车标志
	xp_reset_robot_round_side();
	xp_reset_round_side_leave_car();
	xp_reset_robot_adjust_pose_by_lidar();
	//离车标志复用喷水判断，开始绕边时该标志提前清零，防止下次启动直接喷水
	// isLeaveCar = 0;
	
	//一直等待雷达正常后才开始绕边
	while (1)
	{
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		if(!xp_navi_lidar_status()){
			lidar_open();
			println("v_lidar is offline!");
			xp_upper_v_lidar_state_set(255);
			aos_msleep(1000);
		}else{
			xp_upper_v_lidar_state_set(0);
			break;
		}
	}
	
	start_time = aos_now_ms();
	cur_time = aos_now_ms();
	start_dist = xp_odom_get_distance();
	println("round_side: side %d will round, start_dist %lld", side, start_dist);
	
	while(0 == lidar_navi.robot_round_side_stop){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		loop_cnt++;
		if(cur_time - start_time > timeout){
			robot_stop();
			println("robot round timeout(%d)! start %lld, cur %lld", timeout, start_time, cur_time);
			xp_upper_navi_state_set(1);
			break;
		}
#if (xp_config_enabled(WASH_ROOF_RECTANGULAR) && xp_config_disabled(RECTANGULAR_DRYER_FOLLOW))
		if(true == is_pipe_collision_enable()){
			if(true == check_robot_pipeline_collision(side%4)){
				if(loop_cnt % 5 == 0){
					println("robot pipeline will collision! Pause Stop!");
				}
				robot_stop();
				aos_msleep(100);
				cur_time = aos_now_ms();
				continue;
			}
		}
#else
		//如果与前车的行走距离小于管线架碰撞阈值，则暂停
		if(true == check_robot_pipeline_collision(side%4)){
			if(loop_cnt % 5 == 0){
				println("robot pipeline will collision! Pause Stop!");
			}
			robot_stop();
			aos_msleep(100);
			cur_time = aos_now_ms();
			continue;
		}
#endif
		//如果需要进行陀螺仪软复位，则进行雷达距离方差判断，若超过3.2m也未找平，则放弃
		diff_dist = xp_odom_get_distance() - start_dist;
		if((true == is_soft_reset_gyro_flag()) && (false == b_soft_reset_complete) && (diff_dist < 3200))
		{
			b_no_need_soft_reset = false;
			
			//找平后进行处理，否则继续找
			if(true == is_find_car_parallel())
			{
				robot_angle_soft_reset(side%4);
				//机器人朝向角度软复位后，重新启动里程计--已在原地调整位姿后启动，此处无需再启动
				//xp_odom_start_signal();
				//aos_msleep(1000);
				b_soft_reset_complete = true;
				println("round_side %d: find car parallel, diff_dist %lld", side, diff_dist);
			}
		}else{
			b_no_need_soft_reset = true;

			if(is_wash_car_roof_flag() && false == b_car_y_identify){
				//洗车顶DA边或BC边时获取车辆左右两侧的Y坐标
				ret = xp_joint_get_robot_pose(&p_x, &p_y, NULL);
				if(ret != 0){
					println("round_side: xp_joint_get_robot_pose failed! ret %d", ret);
				}else{
					if(0 == side % 4){
						//DA边找平
						car_right_parallel_y = p_y + LIDAR_Y_BASE_ROBOT_COORDINATE*10 + lidar_navi.robot_safe_distance*10;
						car_right_parallel_x = p_x + LIDAR_X_BASE_ROBOT_COORDINATE*10;
						println("round_side: identify car_right_parallel [%d,%d]", car_right_parallel_x, car_right_parallel_y);
					}else if(2 == side % 4){
						//BC边找平
						car_left_parallel_y = p_y - LIDAR_Y_BASE_ROBOT_COORDINATE*10 - lidar_navi.robot_safe_distance*10;
						car_left_parallel_x = p_x - LIDAR_X_BASE_ROBOT_COORDINATE*10;
						println("round_side: identify car_left_parallel [%d,%d]", car_left_parallel_x, car_left_parallel_y);
					}
					b_car_y_identify = true;
				}
			}
		}

#if xp_config_enabled(WASH_ROOF_RECTANGULAR)
		//洗车顶或者离车调整位姿时，采用找平后直行+直角转弯方式
		if(is_wash_car_roof_flag() || is_leave_adjust_pose_flag())
		{
			if(is_leave_adjust_pose_flag()){
				dist_threshold = 100;
			}else{
				if(0 == side%4 || 2 == side%4){
					//长边
					dist_threshold = ROUND_LONG_SIDE_MIN_DIST;
				}else{
					//短边
					dist_threshold = ROUND_SHORT_SIDE_MIN_DIST;
				}
			}
			
			//每条边直行一段距离后再判断是否离开车
			while((xp_odom_get_distance() - start_dist) < dist_threshold){
				if(get_emergency_status_flag() == true){
					println("====emergency status, exit navi process");
					return -10;
				}

				//在dist_threshold内找平了或不需要找平的情况下（短边）
				if(true == b_no_need_soft_reset){
					println("-1- robot_going_line speed %f, diff_dist %lld threshold %d", lidar_navi.robot_base_speed, (xp_odom_get_distance() - start_dist), dist_threshold);
#ifdef USE_RECTANGULAR_JOINT_MOVE
					if(true == b_need_plan_first_tar_point){
						if(0 == xp_joint_get_robot_pose(&start_x, &start_y, &start_angle)){
							if(0 == side%4 || 2 == side%4){
								//长边按找平后的当前角度规划初始目标点，尽量远一些，等离车后再重新规划最终目标点
								first_tar_point.x = start_x + 5000 * cos(start_angle * DEG2RAD);
								first_tar_point.y = start_y + 5000 * sin(start_angle * DEG2RAD);
								first_tar_point.z = start_angle;
							}else if(1 == side%4 || 3 == side%4){
								//短边按最近一次找平后的角度规划初始目标点，尽量远一些，等离车后再重新规划最终目标点
								first_tar_point.x = start_x + 5000 * cos((robot_angle_zero_offset+90*(side%4)) * DEG2RAD);
								first_tar_point.y = start_y + 5000 * sin((robot_angle_zero_offset+90*(side%4)) * DEG2RAD);
								first_tar_point.z = fmod(robot_angle_zero_offset+90*(side%4), 360);
							}
							println("side %d first_tar_point: [%.1f %.1f %.1f]", side, first_tar_point.x, first_tar_point.y, first_tar_point.z);
							b_need_plan_first_tar_point = false;
						}
					}

					if(false == b_first_point_set_success){
						ret = xp_joint_move_to_point(first_tar_point.x, first_tar_point.y, first_tar_point.z);
					}else{
						if(!xp_joint_is_move_complete()){
							xp_joint_set_motion_speed(lidar_navi.robot_base_speed);
						}
					}
#else
					ret = robot_going_line(lidar_navi.robot_base_speed);
#endif
					if(0 == ret){
#ifdef USE_RECTANGULAR_JOINT_MOVE
						b_first_point_set_success = true;
#endif
						if(move_mode != 2){
							move_mode = 2;
							println("round_side %d: b_front", side);
						}
					}
				}else{
					ret = robot_going();
					if(0 == ret){
						if(move_mode != 1){
							move_mode = 1;
							println("round_side %d: b_going", side);
						}
					}
				}
				if(0 != ret){
					err_cnt++;
					if(err_cnt > 10){
						robot_stop();
						println("robot_front/going(%d) error! ret %d", b_no_need_soft_reset, ret);
						xp_upper_navi_state_set(1);
						return;
					}
				}else{
					err_cnt = 0;
				}
				
				aos_msleep(100);
			}

			//判断是否离开车
			if(0 == lidar_navi.is_robot_round_side_leave_car){
				lidar_dist2 = xp_navi_lidar_get2();
				println("lidar_dist2 %d", lidar_dist2);
				if((lidar_dist2 > ROBOT_CAR_SEARCH_VALUE)){
					//检测到离开车的条件
					leave_detect_count++;
				}else if (0 != leave_detect_count){
					leave_detect_count = 0;
				}
				if(leave_detect_count > 3){
					lidar_navi.is_robot_round_side_leave_car = 1;
					leave_car_start_dist = xp_odom_get_distance();
					println("leave_flag 1, side %d, leave_car_start_dist %lld", side, leave_car_start_dist);
					
					//离车后规划最终目标点
					ret = xp_joint_get_robot_pose(&leave_x, &leave_y, &leave_angle);
					if(0 == side%4 || 2 == side%4){
						//DA边和BC边离车时速度较低，离车判断距离比AB边和CD边多200mm
						leave_dist = 1300;
					}else{
						leave_dist = 1100;
					}
					final_tar_point.x = leave_x + leave_dist * cos(leave_angle * DEG2RAD);
					final_tar_point.y = leave_y + leave_dist * sin(leave_angle * DEG2RAD);
					final_tar_point.z = leave_angle;
					println("side %d final_tar_point: [%.1f %.1f %.1f]", side, final_tar_point.x, final_tar_point.y, final_tar_point.z);
				}

				//在dist_threshold外找平了或不需要找平的情况下（短边）
				if(true == b_no_need_soft_reset){
					println("-2- robot_going_line speed %f", lidar_navi.robot_base_speed);
#ifdef USE_RECTANGULAR_JOINT_MOVE
					if(true == b_need_plan_first_tar_point){
						if(0 == xp_joint_get_robot_pose(&start_x, &start_y, &start_angle)){
							if(0 == side%4 || 2 == side%4){
								//长边按找平后的当前角度规划初始目标点，尽量远一些，等离车后再重新规划最终目标点
								first_tar_point.x = start_x + 5000 * cos(start_angle * DEG2RAD);
								first_tar_point.y = start_y + 5000 * sin(start_angle * DEG2RAD);
								first_tar_point.z = start_angle;
							}else if(1 == side%4 || 3 == side%4){
								//短边按最近一次找平后的角度规划初始目标点，尽量远一些，等离车后再重新规划最终目标点
								first_tar_point.x = start_x + 5000 * cos((robot_angle_zero_offset+90*(side%4)) * DEG2RAD);
								first_tar_point.y = start_y + 5000 * sin((robot_angle_zero_offset+90*(side%4)) * DEG2RAD);
								first_tar_point.z = fmod(robot_angle_zero_offset+90*(side%4), 360);
							}
							println("side %d first_tar_point: [%.1f %.1f %.1f]", side, first_tar_point.x, first_tar_point.y, first_tar_point.z);
							b_need_plan_first_tar_point = false;
						}
					}

					if(false == b_first_point_set_success){
						ret = xp_joint_move_to_point(first_tar_point.x, first_tar_point.y, first_tar_point.z);
					}else{
						if(!xp_joint_is_move_complete()){
							xp_joint_set_motion_speed(lidar_navi.robot_base_speed);
						}
					}
#else
					ret = robot_going_line(lidar_navi.robot_base_speed);
#endif
					if(0 == ret){
#ifdef USE_RECTANGULAR_JOINT_MOVE
						b_first_point_set_success = true;
#endif
						if(move_mode != 2){
							move_mode = 2;
							println("round_side %d: b_front", side);
						}
					}
				}else{
					ret = robot_going();
					if(0 == ret){
						if(move_mode != 1){
							move_mode = 1;
							println("round_side %d: b_going", side);
						}
					}
				}
				if(0 != ret){
					err_cnt++;
					if(err_cnt > 10){
						robot_stop();
						println("robot_front/going(%d) error! ret %d", b_no_need_soft_reset, ret);
						xp_upper_navi_state_set(1);
						return;
					}
				}else{
					err_cnt = 0;
				}
			}else{
#if 1//def USE_RECTANGULAR_JOINT_MOVE
				if(false == b_final_point_set_success){
					if(0 == side%4){
						//洗车顶DA边离车时降速，优化车头刷洗效果
						xp_joint_set_motion_speed(0.1);
					}else if(2 == side%4){
						//洗车顶BC边离车时前半段低速，后半段正常速度
						xp_joint_set_motion_speed(0.03);
					}else{
						//洗车顶其他边离车时高速
						xp_joint_set_motion_speed(0.2);
					}
					ret = xp_joint_move_to_point(final_tar_point.x, final_tar_point.y, final_tar_point.z);
					if(0 == ret){
						b_final_point_set_success = true;
					}
				}else{
					//等待joint_move移动完成
					if(1 == side%4){
						//洗车顶AB边，需等待姿态调整完，因后续动作是相对旋转
						if(xp_joint_is_move_complete()){
							println("round_side %d leave_joint_move to final_tar_point success!", side);
							lidar_navi.robot_round_side_end = 1;
							break;
						}
					}else{
						//其他边，无需等待姿态调整完，只需到达目标点即可，因后续动作是绝对旋转
						if(STA_POSTURE == xp_joint_get_motion_state()){
							if(2 == side%4 || 0 == side%4){
								//如果当前是洗车顶BC边或DA边，绕边结束前需恢复joint_move速度
								xp_joint_set_motion_speed(0.2);
							}
							println("round_side %d leave_joint_move to final_tar_point success!", side);
							lidar_navi.robot_round_side_end = 1;
							break;
						}else{
							if(2 == side%4 && !xp_joint_is_move_complete()){
								//洗车顶BC边离车时前半段低速，后半段正常速度
								if(xp_odom_get_distance() - leave_car_start_dist > ROBOT_LENGTH_VALUE*10*3/5){
									xp_joint_set_motion_speed(0.1);
								}
							}
						}
					}
				}
#else
				if(xp_odom_get_distance() - leave_car_start_dist < leave_car_diff_dist){
					if((2 == side%4) && ((xp_odom_get_distance() - leave_car_start_dist) > ROBOT_LENGTH_VALUE*10/4) && (wait_cnt < 30)){
						//BC边车尾停留3s，方便毛刷压到后挡风玻璃
						robot_stop();
						wait_cnt++;
					}else{
						robot_going_line(0.15);
						if(move_mode != 2){
							move_mode = 2;
							println("round_side %d: b_front", side);
						}
					}
				}else{
					println("cur_dist %lld start_dist %lld, diff_dist > %dmm, robot_stop", xp_odom_get_distance(), leave_car_start_dist, leave_car_diff_dist);
					ret = robot_stop();
					if(0 == ret){
						lidar_navi.robot_round_side_end = 1;
						break;
					}else{
						println("robot_stop failed! ret %d, retry!", ret);
					}
				}
#endif
			}
		}
		else
#endif
		{
			if(0 == side%4 || 2 == side%4){
				//长边
				if(3 == side/4){
					//风机找平后取100即可，太长了会让风机延后判断turn_flag
					dist_threshold = 100;
				}else{
					dist_threshold = ROUND_LONG_SIDE_MIN_DIST;
				}
			}else{
				//短边
				dist_threshold = ROUND_SHORT_SIDE_MIN_DIST;
			}
			
			//每条边绕行一段距离后再判断拐弯
			while((xp_odom_get_distance() - start_dist) < dist_threshold){
				if(get_emergency_status_flag() == true){
					println("====emergency status, exit navi process");
					return -10;
				}

				ret = robot_going();
				if(0 != ret){
					err_cnt++;
					if(err_cnt > 10){
						robot_stop();
						println("robot_going error! ret %d", ret);
						xp_upper_navi_state_set(1);
						return;
					}
				}else{
					err_cnt = 0;
				}
				
				aos_msleep(100);
			}

#ifndef DRYER
			//洗车机洗左侧车顶时,当前坐标靠近车尾x轴坐标一定距离后再开始判拐弯
			if((2 == (side % 4)) && (true == wash_car_roof_flag)){
				while(1){
					if(get_emergency_status_flag() == true){
						println("====emergency status, exit navi process");
						return -10;
					}

					ret = xp_joint_get_robot_pose(&now_x, NULL, NULL);
					if(0 != ret){
						aos_msleep(100);
						continue;
					}

					/*(car_tail_x_position + ROBOT_LENGTH/2 + 100)的位置是雷达测与车尾平行（100为补偿值）
					(Leave_car_dist - 300)保证离车距离至少有300*/
					if(now_x < ((car_tail_x_position + ROBOT_LENGTH/2 + 100) + Leave_car_dist - 300)){
						println("wash roof BC side start identify round side end,car tail x position is %d", car_tail_x_position);
						break;
					}

					ret = robot_going();
					if(0 != ret){
						err_cnt++;
						if(err_cnt > 10){
							robot_stop();
							println("robot_going error! ret %d", ret);
							xp_upper_navi_state_set(1);
							return;
						}
					}else{
						err_cnt = 0;
					}

					aos_msleep(100);
				}
			}
#endif
			
			//判断是否绕边结束
			turn_flag = get_turn_flag(side);
			if(turn_flag){
				println("turn_flag %d, side %d", turn_flag, side);
				robot_stop();
				aos_msleep(1000);
				lidar_navi.robot_round_side_end = 1;
				break;
			}else{
				ret = robot_going();
				if(0 != ret){
					err_cnt++;
					if(err_cnt > 10){
						println("robot_going error! ret %d", ret);
						robot_stop();
						xp_upper_navi_state_set(1);
						return;
					}
				}else{
					err_cnt = 0;
				}
			}
		}
		
		aos_msleep(100);
		cur_time = aos_now_ms();
	}
	
	lidar_navi.robot_round_side_stop = 0;
	println("round_side: side %d round end, end_dist %lld", side, xp_odom_get_distance());
}

void set_round_test_end_flag(bool val)
{
	b_round_test_end = val;
}

/**
 * @brief 雷达绕行调试函数
 * 
 */
void xp_robot_round_test()
{
	int32_t ret = 0;
	uint32_t round_cnt = 0;
	
	xp_gyro_safe_reset();
	xp_odom_start_signal();
	
	while(1){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		if(true == b_round_test_end){
			println("robot round test exit! round_cnt %d", round_cnt);
			break;
		}
		
		xp_set_robot_safe_distance(90);
		xp_robot_round_side(0, ROBOT_ROUND_SIDE_TIME_OUT);
		xp_robot_round_side(1, ROBOT_ROUND_SIDE_TIME_OUT);
		xp_robot_round_side(2, ROBOT_ROUND_SIDE_TIME_OUT);
		
		xp_set_robot_safe_distance(95);
		xp_robot_round_side(3, ROBOT_ROUND_SIDE_TIME_OUT);

		round_cnt++;
		aos_msleep(500);
	}
}
/**
 * @brief 机器人绕边类型
 * 
 */
void xp_set_robot_round_side(int8_t side)
{
	lidar_navi.robot_round_side = side;
}
/**
 * @brief 机器人绕边信号
 * 
 */
void xp_round_side_signal()
{
	aos_sem_signal(&round_side_sem);
}
/**
 * @brief 机器人绕边线程
 * 
 * @param param 线程参数
 */
static void round_side_run(void *param)
{
	while(1){
	   aos_sem_wait(&round_side_sem, AOS_WAIT_FOREVER);
	   println("round_side_run: base_speed %f", lidar_navi.robot_base_speed);
	   xp_robot_round_side(lidar_navi.robot_round_side, ROBOT_ROUND_SIDE_TIME_OUT);
   }
}

/**
 * @brief 机器人绕边信号
 * 
 */
void xp_round_side_reset_gyro_signal()
{
	aos_sem_signal(&round_side_reset_gyro_sem);
}

/**
 * @brief 机器人绕边信号
 * 
 */
void xp_round_side_dist_reset_gyro_signal()
{
	aos_sem_signal(&round_side_dist_reset_gyro_sem);
}

void set_round_side_reset_gyro_timeout(int32_t val)
{
	round_side_reset_gyro_timeout = val;
}
void set_round_side_reset_gyro_dist(int32_t val)
{
	round_side_reset_gyro_dist = val;
}

uint8_t is_gyro_reset_start = false;
/**
 * @brief 机器人绕边复位陀螺仪线程
 * 
 * @param param 线程参数
 */
static void round_side_reset_gyro_run(void *param)
{
	while(1){
	   aos_sem_wait(&round_side_reset_gyro_sem,AOS_WAIT_FOREVER);
	   is_gyro_reset_start = true;
	   println("xp_robot_round_side_time_reset_gyro......");
	   xp_robot_round_side_time_reset_gyro(round_side_reset_gyro_timeout);
   }
}
static void round_side_reset_gyro_run_test(void *param)
{
	while(1){
	   if (is_gyro_reset_start){
			xp_robot_round_side_time_reset_gyro_test(round_side_reset_gyro_timeout);
			is_gyro_reset_start = false;
	   }
	   aos_msleep(100);
   }
}

/**
 * @brief 机器人绕边一段距离后复位陀螺仪线程
 * 
 * @param param 线程参数
 */
static void round_side_dist_reset_gyro_run(void *param)
{
	while(1){
	   aos_sem_wait(&round_side_dist_reset_gyro_sem, AOS_WAIT_FOREVER);
	   println("xp_robot_round_side_dist_reset_gyro......");
	   xp_robot_round_side_dist_reset_gyro(round_side_reset_gyro_dist);
   }
}

void xp_adjust_pose_by_lidar_signal()
{
	aos_sem_signal(&adjust_pose_by_lidar_sem);
}

void xp_leave_adjust_pose_by_lidar_signal()
{
	aos_sem_signal(&leave_adjust_pose_by_lidar_sem);
}

void set_adjust_pose_by_lidar_timeout(int32_t val)
{
	adjust_pose_by_lidar_timeout = val;
}

bool xp_is_robot_adjust_pose_by_lidar_end()
{
	return adjust_pose_by_lidar_end_flag;
}

void xp_reset_robot_adjust_pose_by_lidar()
{
	adjust_pose_by_lidar_end_flag = false;
}

static void adjust_pose_by_lidar_run(void *param)
{
	while(1){
	   aos_sem_wait(&adjust_pose_by_lidar_sem, AOS_WAIT_FOREVER);
	   println("xp_robot_adjust_pose_by_lidar_time......");
	   xp_robot_adjust_pose_by_lidar_time(adjust_pose_by_lidar_timeout);
   }
}

static void leave_adjust_pose_by_lidar_run(void *param)
{
	while(1){
	   aos_sem_wait(&leave_adjust_pose_by_lidar_sem, AOS_WAIT_FOREVER);
	   println("xp_leave_adjust_pose_by_lidar_time......");
	   xp_leave_adjust_pose_by_lidar_time(adjust_pose_by_lidar_timeout);
   }
}

/**
 * @brief 机器人根据雷达检测调整位姿
 * 
 * @param time 调整位姿的超时时间，单位ms
 */
int32_t xp_robot_adjust_pose_by_lidar_time(int32_t timeout)
{
	int32_t ret = 0;
	uint8_t wait_cnt = 0, max_wait_cnt = 20;
	uint8_t get_cnt = 0, identify_cnt = 0, loop_cnt = 0;
	bool b_identify = false;
	bool b_force_identify = false;
	const LIDAR_angle_point *anglePoint = xp_linux_lidar_anglePoint_get();
	LIDAR_angle_point org_ap;//感知算法提供的原始点，基于雷达坐标系，雷达正前方为X，左侧为Y
	LIDAR_angle_point rot_inv90_ap;//原始坐标系顺时针旋转90度后的端点坐标，该坐标系与机器人坐标系平行
	LIDAR_angle_point move_center_ap;//将旋转后的坐标系平移到机器人中心点上后的端点坐标
	LIDAR_angle_point rot_straight_ap;//将机器人中心点的坐标系摆正后的端点坐标
	float line_length = 0;//两端点直线长度
	float slope = 0;//两端点斜率
	float theta_rad = 0;//机器人与车身的角度，单位弧度
	int32_t theta = 0;//机器人与车身的角度，单位1/1000度
	int32_t turn_theta = 0;//调整距离前机器人需要转动的角度，避免角度太小距离调整太长，单位1/1000度
	int32_t rot_straight_dist  = 0;//机器人摆正后雷达与车身的距离，单位cm
	int32_t move_time = 0;
	float move_speed = 0.15;
	float axis_y = 0;
	int32_t lidar_dist = 0, median_dist = 0;
	bool b_need_back_adjust = false;
	bool b_need_identify_60cm = false;//后续需要识别60cm的直线
	
#if 1
RETRY:
	//先测量离车距离并进行过近判断
	xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);
	aos_msleep(500);
	b_need_back_adjust = false;
	median_dist = 0;
	get_cnt = 0;
	do{
		lidar_dist = xp_navi_lidar_get();
		median_dist += lidar_dist;
		get_cnt++;
		aos_msleep(200);
	}while(get_cnt < 3);
	median_dist = median_dist / get_cnt;
	if(median_dist < 77){
		println("dist(%d) between car and robot is too near! Can't rotate to identify!", median_dist);
		return -7;
	}

	do{
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		//通知linux主板识别车身直线端点
		xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_ANGLE);

		wait_cnt = 0;
		if(0 == identify_cnt){
			max_wait_cnt = 10;//平行时找车最多等5s
		}else{
			max_wait_cnt = 20;//斜时找车最多等10s
		}
		do{
			if(get_emergency_status_flag() == true){
				println("====emergency status, exit navi process");
				return -10;
			}

			aos_msleep(500);
			wait_cnt++;
			memcpy(&org_ap, anglePoint, sizeof(LIDAR_angle_point));
			line_length = sqrt(pow(org_ap.p2.x - org_ap.p1.x, 2) + pow(org_ap.p2.y - org_ap.p1.y, 2));
			if((false == b_need_identify_60cm && line_length - 80 > 0) || (true == b_need_identify_60cm && line_length - 60 > 0)){
				b_identify = true;
				xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);
				println("identify anglePoint success! p2(%d,%d) p1(%d,%d)", org_ap.p2.x, org_ap.p2.y, org_ap.p1.x, org_ap.p1.y);
				break;
			}
			
			if(wait_cnt > max_wait_cnt){
				//超过10s未识别，再顺时针旋转10度进行识别
				b_identify = false;
				break;
			}
		}while(1);

		//切回正常测距模式
		xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);

		if(true == b_identify){
			break;
		}else{
			println("identify anglePoint failed! Loop %d identify_cnt %d p1[%d,%d] p2[%d,%d] line_length %.3f b_need_identify_60cm %d", 
				loop_cnt, identify_cnt, org_ap.p1.x, org_ap.p1.y, org_ap.p2.x, org_ap.p2.y, line_length, b_need_identify_60cm);
			identify_cnt++;
			if(identify_cnt > 3){
				//第一轮未识别到80cm以上的直线，第二轮开始就识别60cm以上的
				b_need_identify_60cm = true;
				loop_cnt++;
				if(loop_cnt > 2){
					// //如果3个大循环都没有识别成功，退出洗车流程
					// println("identify anglePoint %d loops failed!!! Exit!", loop_cnt);
					// return -2;

					//三个大循环未找到车，不再找车，继续后续流程
					b_force_identify = true;
					xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);
					println("identify anglePoint %d loops failed!!! force success!", loop_cnt);
					break;
				}else{
					//当前循环没有识别成功，调整下再进行下一轮识别
					//后退角度不能过大，以免撞到车，先转回10度再移动调整
					xp_local_turn(10000);
					if(median_dist < 85){
						//机器可能离车太近导致无法识别，往外靠一点再进行下一轮识别
						move_time = (int32_t)((100 - median_dist)*10 / sin(10*(identify_cnt-2)*DEG2RAD) / move_speed);
						println("loop %d identify failed! adjust_pose: median_dist %d robot_front %dms, speed %f", loop_cnt, median_dist, move_time, move_speed);
						robot_front(move_speed);
						aos_msleep(move_time);
						robot_stop();
					}else if(median_dist > 120){
						//机器可能离车太远导致无法识别，往里靠一点再进行下一轮识别
						move_time = (int32_t)((median_dist - 110)*10 / sin(10*(identify_cnt-2)*DEG2RAD) / move_speed);
						println("loop %d identify failed! adjust_pose: median_dist %d robot_back %dms, speed %f", loop_cnt, median_dist, move_time, move_speed);
						robot_back(move_speed);
						aos_msleep(move_time);
						robot_stop();
					}else{
						//机器离车距离在合适范围内，但还是未识别，可能是识别的直线长度不够，转正后往后靠一点再进行下一轮识别
						b_need_back_adjust = true;
					}
					
					//当前循环没有识别成功，转回到原来角度重新开始，如果3个大循环都没有识别成功，退出洗车流程
					xp_local_turn(10000*(identify_cnt-2) + 1500*(identify_cnt-3));
					if(true == b_need_back_adjust){
						println("loop %d identify failed! adjust_pose: robot_back 1500ms, speed %f", loop_cnt, move_speed);
						robot_back(move_speed);
						aos_msleep(1500);
						robot_stop();
					}
					
					//超过3次未识别则重新一个大循环
					println("identify anglePoint %d times failed!!! Retry loop %d!", identify_cnt, loop_cnt);
					identify_cnt = 0;
					goto RETRY;
				}
			}
			
			//顺时针旋转10度再进行识别
			ret = xp_local_turn(-10000);
			if(ret < 0){
				println("xp_local_turn failed! ret %d", ret);
				return -3;
			}
		}
	}while(1);
#else
	org_ap.p2.x = org_x2;
	org_ap.p2.y = org_y2;
	org_ap.p1.x = org_x1;
	org_ap.p1.y = org_y1;
#endif
	if(b_identify == true){
		//根据识别到的端点计算机器人与车身的角度和转正后的垂直距离
		rot_inv90_ap.p2.x = -org_ap.p2.y;
		rot_inv90_ap.p2.y = org_ap.p2.x;
		rot_inv90_ap.p1.x = -org_ap.p1.y;
		rot_inv90_ap.p1.y = org_ap.p1.x;
		slope = (float)(rot_inv90_ap.p2.y - rot_inv90_ap.p1.y) / (float)(rot_inv90_ap.p2.x - rot_inv90_ap.p1.x);
		theta_rad = atan(slope);
		theta = (int32_t)(theta_rad * 180000 / PI);
		move_center_ap.p2.x = rot_inv90_ap.p2.x + LIDAR_X_BASE_ROBOT_COORDINATE;
		move_center_ap.p2.y = rot_inv90_ap.p2.y + LIDAR_Y_BASE_ROBOT_COORDINATE;
		move_center_ap.p1.x = rot_inv90_ap.p1.x + LIDAR_X_BASE_ROBOT_COORDINATE;
		move_center_ap.p1.y = rot_inv90_ap.p1.y + LIDAR_Y_BASE_ROBOT_COORDINATE;
		rot_straight_ap.p2.x = cos(theta_rad) * move_center_ap.p2.x + sin(theta_rad) * move_center_ap.p2.y;
		rot_straight_ap.p2.y = -sin(theta_rad) * move_center_ap.p2.x + cos(theta_rad) * move_center_ap.p2.y;
		rot_straight_ap.p1.x = cos(theta_rad) * move_center_ap.p1.x + sin(theta_rad) * move_center_ap.p1.y;
		rot_straight_ap.p1.y = -sin(theta_rad) * move_center_ap.p1.x + cos(theta_rad) * move_center_ap.p1.y;
		rot_straight_dist = rot_straight_ap.p2.y - LIDAR_Y_BASE_ROBOT_COORDINATE;
		println("org_ap: p2(%d,%d), p1(%d,%d); rot_inv90_ap: p2(%d,%d), p1(%d,%d); slope %.1f th_rad %.1f theta %d",
			org_ap.p2.x, org_ap.p2.y, org_ap.p1.x, org_ap.p1.y,
			rot_inv90_ap.p2.x, rot_inv90_ap.p2.y, rot_inv90_ap.p1.x, rot_inv90_ap.p1.y,
			slope, theta_rad, theta);
		println("move_center_ap: p2(%d,%d), p1(%d,%d); rot_straight_ap: p2(%d,%d), p1(%d,%d); rot_straight_dist %d",
			move_center_ap.p2.x, move_center_ap.p2.y, move_center_ap.p1.x, move_center_ap.p1.y,
			rot_straight_ap.p2.x, rot_straight_ap.p2.y, rot_straight_ap.p1.x, rot_straight_ap.p1.y, rot_straight_dist);

		//如果摆正后雷达与车身的距离大于260cm或小于60cm，则认为异常
		if(rot_straight_dist > 260 || rot_straight_dist < 60){
			println("robot_adjust_pose: rot_straight_dist %d NOT normal!!! Exit adjust pose!", rot_straight_dist);
			return -4;
		}

		//如果当前角度偏差小于20度，机器人需要先转到-20度，避免调整距离过长一直调不到位
		if(theta < 20000){
			turn_theta = -20000 + theta;
			println("adjust_pose: before turn, theta %d turn_theta %d, theta_rad %f", theta, turn_theta, theta_rad);
			ret = xp_local_turn(turn_theta);
			if(ret < 0){
				println("xp_local_turn %d failed! ret %d", turn_theta, ret);
				return -5;
			}
			theta = 20000;
			theta_rad = PI * 20.0 / 180.0;
			println("adjust_pose: after turn, theta_rad %f", theta_rad);
		}

#if 0
		//使用管线架编码器方式运动到目标点
		int32_t x, y, tar_x, tar_y;
		float angle, tar_angle;
		int32_t tar_center_leave_car_y;//机器人目标中心点离车的Y轴距离，单位cm
		int32_t cur_center_leave_tar_y;//机器人当前中心点离目标中心点的Y轴距离，单位cm
		int32_t tar_center_leave_cur_center_x;//机器人目标中心点离当前中心点的X轴距离，单位cm
		xp_joint_get_robot_pose(&x, &y, &angle);
		tar_center_leave_car_y = 95 - LIDAR_Y_BASE_ROBOT_COORDINATE;//|CD|
		cur_center_leave_tar_y = rot_straight_ap.p2.y - tar_center_leave_car_y;//|AC|
		tar_center_leave_cur_center_x = cur_center_leave_tar_y / tan((360-angle) * DEG2RAD);//|BC|
		println("adjust_pose: tar_center_leave_car_y %d cur_center_leave_tar_y %d tar_center_leave_cur_center_x %d", 
			tar_center_leave_car_y, cur_center_leave_tar_y, tar_center_leave_cur_center_x);
		tar_x = x - tar_center_leave_cur_center_x*10;
		tar_y = y + cur_center_leave_tar_y*10;
		//先直线移动到目标点
		println("adjust_pose: -1- robot joint move from (%d,%d,%f) to (%d,%d,%f)", x, y, angle, tar_y, tar_x, angle);
		xp_joint_move_to_point(tar_x, tar_y, angle);
		while( !xp_joint_is_move_complete() ){
			aos_msleep(100);
		}
		//xp_joint_get_robot_pose(&x, &y, &angle);//不更新，按原计算值进行转动
		tar_angle = theta / 1000 - (360 - angle) + 180;//+180是因为给管线架输入的角度需要按管线架定义的方式来转换
		//再原地旋转到目标角度
		println("adjust_pose: -2- robot joint move from (%d,%d,%f) to (%d,%d,%f)", x, y, angle, tar_y, tar_x, tar_angle);
		xp_joint_move_to_point(tar_x, tar_y, tar_angle);
		while( !xp_joint_is_move_complete() ){
			aos_msleep(100);
		}
#else	//使用延时方式运动到目标点
		//如果摆正后的垂直距离过小或过大，需要先前进后退调整至合适距离
		if(rot_straight_dist < 90){
			move_time = (int32_t)((90 - rot_straight_dist)*10 / sin(theta_rad) / move_speed) + 200;//200ms为加减速补偿时间
			println("adjust_pose: will robot_front %dms, speed %f", move_time, move_speed);
			robot_front(move_speed);
			aos_msleep(move_time);
			robot_stop();
		}else if(rot_straight_dist > 110){
			move_time = (int32_t)((rot_straight_dist - 110)*10 / sin(theta_rad) / move_speed) + 200;//200ms为加减速补偿时间
			println("adjust_pose: will robot_back %dms, speed %f", move_time, move_speed);
			robot_back(move_speed);
			aos_msleep(move_time);
			robot_stop();
		}

		//调整至合适距离进行摆正
		ret = xp_local_turn(theta);
		if(ret < 0){
			println("xp_local_turn failed! ret %d", ret);
			return -6;
		}
#endif
	}else{
		ret = xp_local_turn(10000*(identify_cnt-1) + 1500*(identify_cnt-2));		//转回原来的角度,1500为补偿角度(单次旋转都有1~2度的误差)
		if(ret < 0){
			println("xp_local_turn failed! ret %d", ret);
			return -7;
		}
	}


#if 0		//位置调整结束后洗车机可能已经离开车，导致流程中断，暂时先屏蔽
	//调整结束后再次进行离车距离的检查，避免因编码器异常等原因导致调整错误
	median_dist = 0;
	get_cnt = 0;
	do{
		lidar_dist = xp_navi_lidar_get();
		median_dist += lidar_dist;
		get_cnt++;
		aos_msleep(200);
	}while(get_cnt < 3);
	median_dist = median_dist / get_cnt;
	if(median_dist > 130){
		println("dist(%d) between car and robot is disnormal after pose adjust!", median_dist);
		return -8;
	}
#endif
	
	//进行机器人朝向角度软复位，并启动里程计
	robot_angle_soft_reset(0);
	xp_odom_start_signal();
	aos_msleep(1000);

	//设置调整位姿结束标志
	adjust_pose_by_lidar_end_flag = true;

	return 0;
}

/**
 * @brief 机器人离开车头后再根据雷达检测调整位姿
 * 
 * @param time 调整位姿的超时时间，单位ms
 */
int32_t xp_leave_adjust_pose_by_lidar_time(int32_t timeout)
{
	int32_t ret = 0;
	uint8_t wait_cnt = 0;
	uint8_t identify_cnt = 0;
	bool b_identify = false;
	const LIDAR_angle_point *anglePoint = xp_linux_lidar_anglePoint_get();
	LIDAR_angle_point org_ap;//感知算法提供的原始点，基于雷达坐标系，雷达正前方为X，左侧为Y
	LIDAR_angle_point rot_inv90_ap;//原始坐标系顺时针旋转90度后的端点坐标，该坐标系与机器人坐标系平行
	LIDAR_angle_point move_center_ap;//将旋转后的坐标系平移到机器人中心点上后的端点坐标
	LIDAR_angle_point rot_straight_ap;//将机器人中心点的坐标系摆正后的端点坐标
	float slope = 0;//两端点斜率
	float theta_rad = 0;//机器人与车身的角度，单位弧度
	int32_t theta = 0;//机器人与车身的角度，单位1/1000度
	int32_t rot_straight_dist  = 0;//机器人摆正后雷达与车身的距离，单位cm
	int32_t move_time = 0;
#if 1
	set_leave_adjust_pose_flag(true);

	//陀螺仪软复位后开始沿AB边前进
	gyro_reset_fun(1);
	aos_msleep(1000);
	xp_robot_round_side(1, 100000);
	
	//确认反光镜已收回再旋转
	while(MIRROR_close != xp_mirrors_get_state()){
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		println("leave_adjust_pose: wait mirrors close...");
		aos_msleep(500);
	}

	//清空识别标志
	xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);

	//先原地逆时针转60度
	ret = xp_local_turn(60000);
	if(ret < 0){
		println("xp_local_turn failed! ret %d", ret);
		return -1;
	}

	do{
		if(get_emergency_status_flag() == true){
			println("====emergency status, exit navi process");
			return -10;
		}

		//通知linux主板识别车身直线端点
		xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_ANGLE);

		wait_cnt = 0;
		do{
			if(get_emergency_status_flag() == true){
				println("====emergency status, exit navi process");
				return -10;
			}

			aos_msleep(1000);
			wait_cnt++;
			memcpy(&org_ap, anglePoint, sizeof(LIDAR_angle_point));
			if(org_ap.p2.x != org_ap.p1.x){
				b_identify = true;
				xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);
				println("identify anglePoint success! p2(%d,%d) p1(%d,%d)", org_ap.p2.x, org_ap.p2.y, org_ap.p1.x, org_ap.p1.y);
				break;
			}
			if(wait_cnt > 5){
				//超过5s未识别，再顺时针旋转10度进行识别
				b_identify = false;
				break;
			}
		}while(1);

		if(true == b_identify){
			break;
		}else{
			identify_cnt++;
			if(identify_cnt > 3){
				//超过3次未识别则退出后续流程
				println("identify anglePoint %d times failed!!! Exit!", identify_cnt);
				return -2;
			}

			println("identify anglePoint failed! Retry %d", identify_cnt);
			
			//清空识别标志
			xp_linux_lidar_doAngle_set(LIDAR_MODE_MEASURE_DIST);
			
			//顺时针旋转10度再进行识别
			ret = xp_local_turn(-10000);
			if(ret < 0){
				println("xp_local_turn failed! ret %d", ret);
				return -3;
			}
		}
	}while(1);
#else
	org_ap.p2.x = org_x2;
	org_ap.p2.y = org_y2;
	org_ap.p1.x = org_x1;
	org_ap.p1.y = org_y1;
#endif
	//根据识别到的端点计算机器人与车身的角度和转正后的垂直距离
	rot_inv90_ap.p2.x = -org_ap.p2.y;
	rot_inv90_ap.p2.y = org_ap.p2.x;
	rot_inv90_ap.p1.x = -org_ap.p1.y;
	rot_inv90_ap.p1.y = org_ap.p1.x;
	slope = (float)(rot_inv90_ap.p2.y - rot_inv90_ap.p1.y) / (float)(rot_inv90_ap.p2.x - rot_inv90_ap.p1.x);
	theta_rad = atan(slope);
	theta = (int32_t)(theta_rad * 180000 / PI);
	move_center_ap.p2.x = rot_inv90_ap.p2.x + LIDAR_X_BASE_ROBOT_COORDINATE;
	move_center_ap.p2.y = rot_inv90_ap.p2.y + LIDAR_Y_BASE_ROBOT_COORDINATE;
	move_center_ap.p1.x = rot_inv90_ap.p1.x + LIDAR_X_BASE_ROBOT_COORDINATE;
	move_center_ap.p1.y = rot_inv90_ap.p1.y + LIDAR_Y_BASE_ROBOT_COORDINATE;
	rot_straight_ap.p2.x = cos(theta_rad) * move_center_ap.p2.x + sin(theta_rad) * move_center_ap.p2.y;
	rot_straight_ap.p2.y = -sin(theta_rad) * move_center_ap.p2.x + cos(theta_rad) * move_center_ap.p2.y;
	rot_straight_ap.p1.x = cos(theta_rad) * move_center_ap.p1.x + sin(theta_rad) * move_center_ap.p1.y;
	rot_straight_ap.p1.y = -sin(theta_rad) * move_center_ap.p1.x + cos(theta_rad) * move_center_ap.p1.y;
	rot_straight_dist = rot_straight_ap.p2.y - LIDAR_Y_BASE_ROBOT_COORDINATE;
	println("org_ap: p2(%d,%d), p1(%d,%d); rot_inv90_ap: p2(%d,%d), p1(%d,%d); slope %.1f th_rad %.1f theta %d",
		org_ap.p2.x, org_ap.p2.y, org_ap.p1.x, org_ap.p1.y,
		rot_inv90_ap.p2.x, rot_inv90_ap.p2.y, rot_inv90_ap.p1.x, rot_inv90_ap.p1.y,
		slope, theta_rad, theta);
	println("move_center_ap: p2(%d,%d), p1(%d,%d); rot_straight_ap: p2(%d,%d), p1(%d,%d); rot_straight_dist %d",
		move_center_ap.p2.x, move_center_ap.p2.y, move_center_ap.p1.x, move_center_ap.p1.y,
		rot_straight_ap.p2.x, rot_straight_ap.p2.y, rot_straight_ap.p1.x, rot_straight_ap.p1.y, rot_straight_dist);

	//如果摆正后的垂直距离过小或过大，需要先前进后退调整至合适距离
	if(rot_straight_dist < 90){
		move_time = (int32_t)((90 - rot_straight_dist)*10 / sin(theta_rad) / 0.1) + 200;//200ms为加减速补偿时间
		println("adjust_pose: will robot_front %dms", move_time);
		robot_front(0.1);
		aos_msleep(move_time);
		robot_stop();
	}else if(rot_straight_dist > 100){
		move_time = (int32_t)((rot_straight_dist - 100)*10 / sin(theta_rad) / 0.1) + 200;//200ms为加减速补偿时间
		println("adjust_pose: will robot_back %dms", move_time);
		robot_back(0.1);
		aos_msleep(move_time);
		robot_stop();
	}

	//调整至合适距离进行摆正
	ret = xp_local_turn(theta);
	if(ret < 0){
		println("xp_local_turn failed! ret %d", ret);
		return -4;
	}

	//进行机器人朝向角度软复位，并启动里程计
	robot_angle_soft_reset(2);
	xp_odom_start_signal();
	aos_msleep(1000);

	//设置调整位姿结束标志
	adjust_pose_by_lidar_end_flag = true;
	set_leave_adjust_pose_flag(false);

	return 0;
}

static void xp_leave_car(void *arg){
	
	while(1){
		aos_sem_wait(&leave_sem,AOS_WAIT_FOREVER);
		
		if ('B' == leave_car_type || 'D' == leave_car_type){
			xp_robot_leave_car_B_D();
		}else if ('A' == leave_car_type){
			xp_robot_leave_car_A();
		}else if('C' == leave_car_type){
			xp_robot_leave_car_C();
		}
	}
	
}
void xp_leave_sem_set(uint8_t type){
	leave_car_type = type;
	isLeaveCar = 0;
	aos_sem_signal(&leave_sem);
}
/**
 * @brief 启动里程计监测线程
 * 
 * @param param 线程参数
 */
static void start_odom_monitor_thread(void *param){
	println("enter!");
	xp_odom_enable(1);
	
	while(1)
	{
		aos_sem_wait(&start_odom_sem, AOS_WAIT_FOREVER);
		
		if (xp_is_odom_enabled())
		{
			//先停止上一次的里程计计算
			xp_odom_stop();
			//1s后启动新一次的里程计计算
			println("start odom after 1 seconds");
			aos_msleep(1000);
			xp_odom_start();
		}
		else
		{
			// println("odom is NOT enabled! Don't start");
		}
		aos_msleep(100);
	}
}
/**
 * @brief 机器人拐弯类型
 * 
 */
void xp_set_robot_turning_type(int8_t type)
{
	lidar_navi.robot_turning_type = type;
}
/**
 * @brief 机器人拐弯信号
 * 
 */
void xp_turning_signal()
{
	//aos_sem_signal(&turning_sem);
	b_turning = true;
}
/**
 * @brief 机器人拐弯线程
 * 
 * @param param 线程参数
 */
static void turning_run(void *param)
{
	while(1){
	   //aos_sem_wait(&turning_sem,AOS_WAIT_FOREVER);
	   if(true == b_turning){
	       xp_robot_turning(lidar_navi.robot_turning_type);
		   b_turning = false;
	   }
	   aos_msleep(50);
   }
}
/**
 * @brief 雷达导航参数初始化
 * 
 * @param plidar_navigation 雷达导航参数
 */
static void lidar_navigation_param_init(lidar_navigation_object *plidar_navigation)
{
	plidar_navigation->robot_safe_distance = 90;
	plidar_navigation->kp = 0.003;
	plidar_navigation->ki = 0;
	plidar_navigation->kd = 0;
	plidar_navigation->robot_base_speed = 0.15;
	plidar_navigation->robot_speed_max = 0.32;
	plidar_navigation->robot_pid_offset = 0;
	plidar_navigation->left_upper_io_num = 3;
	plidar_navigation->right_upper_io_num = 4;
	plidar_navigation->robot_round_side_end = 0;
	plidar_navigation->robot_round_side_stop = 0;
	plidar_navigation->robot_turning_end = 0;
	
	plidar_navigation->lidar_open_flag = 0;
	plidar_navigation->is_robot_straightening_end = 0;
}

/**
 * @brief 重新开始洗车时，初始化所有需要重新赋初始值的参数
 */
int32_t lidar_navigation_recover_wash_param_init()
{
	lidar_navi.kp = 0.003;
	lidar_navi.ki = 0;
	lidar_navi.kd = 0;
	lidar_navi.robot_base_speed = 0.15;
	lidar_navi.robot_speed_max = 0.32;
	lidar_navi.robot_pid_offset = 0;
	lidar_navi.mean_square_error_value = 100;
	lidar_navi.mean_square_index = 0;
	lidar_navi.robot_safe_distance = 90;
	lidar_navi.robot_round_side_end = 0;
	lidar_navi.robot_turning_end = 0;
	lidar_navi.robot_round_side_stop = 0;
	lidar_navi.is_robot_straightening_end = 0;
	lidar_navi.is_robot_round_side_leave_car = 0;
	lidar_navi.b_odom_enable = 1;						//默认允许启用里程计

	wash_car_roof_flag = 0;
	soft_reset_gyro_flag = 0;
	dist_filter_flag = 0;
	leave_adjust_pose_flag = 0;

	b_round_test_end = false;
	b_turning = false;
	robot_angle_zero_offset = 0;
	turn_angle = 20;
	pid_turn_angle = 30;

	round_side_reset_gyro_flag = 0;
	round_side_reset_gyro_timeout = 30000;
	round_side_reset_gyro_dist = 3000;
	dist_reset_gyro_round_side_end_flag = 0;
	car_head_x_position = 0;
	car_tail_x_position = 0;
	car_left_parallel_y = 0;
	car_left_parallel_x = 0;
	car_right_parallel_y = 0;
	car_right_parallel_x = 0;
	// is_emergency_status = true;							//默认处于急停状态，等待检测到一定时间无急停信号后恢复
	adjust_pose_by_lidar_timeout = 60000;
	adjust_pose_by_lidar_end_flag = false;

	FindCarMileage = 0;
	FindCarDistance = 0;
	isFindCarComplete = false;
	isLeaveCar = 0;
	leave_car_type = 0;
	last_pid_out = 0;

	b_detect_pipe_collision = false;
	pipe_collision_enable_flag = false;
}

/**
 * @brief 雷达导航初始化
 * 
 * @return int32_t 0表示初始化成功，非0则失败。
 */
int32_t xp_lidar_navigation_init()
{
	int ret = -1;

	lidar_navigation_param_init(&lidar_navi);
	
	ret = lidar_open();
	if(0 != ret)
	{
		println("lidar_open failed! ret %d", ret);
		return -1;
	}
#ifndef WASHER_MERGE
	ret = xp_gyro_init();
	if(0 != ret)
	{
		println("gyro_init failed! ret %d", ret);
		return -1;
	}
#endif
	ret = PIDx_init(&(lidar_navi.pid));
	if(0 != ret)
	{
		println("PIDx_init failed! ret %d", ret);
		return -1;
	}
		
	//后续需要调节p i d的值来使AGV稳定行走
	ret = PIDx_setparam(&(lidar_navi.pid), lidar_navi.kp, lidar_navi.ki, lidar_navi.kd);
	if(0 != ret)
	{
		println("PIDx_setparam failed! ret %d", ret);
		return -1;
	}
		
	ret = PIDx_setvalue(&(lidar_navi.pid), lidar_navi.robot_safe_distance);
	if(0 != ret)
	{
		println("PIDx_setvalue failed! ret %d", ret);
		return -1;
	}
#ifndef WASHER_MERGE
#ifdef LINE_VERSION
    ret = xp_wheel_init(1,115200,10);
#elif defined BATTERY_VERSION
	ret = xp_wheel_init(2,1,115200);
#endif
	if(ret != 0)
	{	println("wheel_init failed! ret %d", ret);
		return -1;
	}
#endif
	aos_sem_new(&leave_sem,0);
	if (0 != aos_task_new("leave_car", xp_leave_car, NULL, 2048)) {
		printf("create leave_car thread fail");
		return -1;
	}
	aos_sem_new(&turning_sem,0);
	if (0 != aos_task_new("turning_run", turning_run, NULL, 2048)) {
		printf("create turning_run thread fail");
		return -1;
	}
	aos_sem_new(&round_side_sem,0);
	/* round_side_run线程栈空间采用8KB，较小的情况下会导致HardFault异常，应该是内存溢出引起 */
	if (0 != aos_task_new("round_side", round_side_run, NULL, 2048)) {
		printf("create round_side_run thread fail");
		return -1;
	}
	/*aos_sem_new(&round_side_reset_gyro_sem,0);
	if (0 != aos_task_new("round_side_reset_gyro", round_side_reset_gyro_run, NULL, 2048)) {
		printf("create round_side_reset_gyro_run thread fail");
		return -1;
	}*/
	aos_sem_new(&round_side_dist_reset_gyro_sem,0);
	if (0 != aos_task_new("dist_reset_gyro", round_side_dist_reset_gyro_run, NULL, 2048)) {
		printf("create round_side_dist_reset_gyro_run thread fail");
		return -1;
	}
#if xp_config_enabled(WASH_ROOF_RECTANGULAR)
	aos_sem_new(&leave_adjust_pose_by_lidar_sem,0);
	if (0 != aos_task_new("leave_adjust_pose", leave_adjust_pose_by_lidar_run, NULL, 2048)) {
		printf("create leave_adjust_pose_by_lidar_run thread fail");
		return -1;
	}
#else
	aos_sem_new(&adjust_pose_by_lidar_sem,0);
	if (0 != aos_task_new("adjust_pose", adjust_pose_by_lidar_run, NULL, 2048)) {
		printf("create adjust_pose_by_lidar_run thread fail");
		return -1;
	}
#endif
	aos_sem_new(&robot_straightening_sem,0);
	if (0 != aos_task_new("robot_straightening", xp_robot_straightening_run, NULL, 2048)) {
		printf("create robot_straightening_run thread fail");
		return -1;
	}
	/* 创建启动里程计计算的线程 */
	aos_sem_new(&start_odom_sem,0);
	if (0 != aos_task_new("start_odom_monitor", start_odom_monitor_thread, NULL, 2048)) {
		printf("create start_odom_monitor_thread fail");
		return -1;
	}
	/* 创建周期为200ms的里程计计算定时器，不自动运行，需在陀螺仪复位时启动 */
	ret = aos_timer_new_ext(&odom_calc_timer, xp_odom_calc, NULL, ROBOT_ODOM_CALC_PERIOD, 1, 0);
	if (ret != 0){
		printf("create odom_calc_timer failed!\n");
		return -2;
	}
	
	/* 机器人运动控制初始化 */
	xp_motion_init();
	
	println("xp_lidar_navigation_init success~~~");
	
	return 0;
}

int xp_navigation_debug(char *type,char *fun,char *param){
    if(strcmp(type,"navi")!=0)return 0;
    if(fun==NULL)return 0;
    print("navi debug\r\n");
	if(strcmp(fun,"set_k")==0){
		print("Modif K:%f->%f\r\n", f_K, atof(param));
		f_K = atof(param);
	}
	else if(strcmp(type,"set_corner_speed")==0){
		corner_move_s=atof(param);
		println("set corner new speed: %0.2f",corner_move_s);
	}
    else if(strcmp(fun,"set_kp")==0){
		print("Modif Kp: %f->%f\r\n", lidar_navi.pid.Kp, atof(param));
		lidar_navi.pid.Kp = atof(param);
	}
	else if (strcmp(fun, "Angle1") == 0)
	{
		print("Modif Angle1: %d->%d\r\n", Angle_1, atoi(param));
		Angle_1 = atoi(param);
	}
	else if (strcmp(fun, "Angle2") == 0)
	{
		print("Modif Angle2: %d->%d\r\n", Angle_2, atoi(param));
		Angle_2 = atoi(param);
	}
	else if (strcmp(fun, "allow_range") == 0)
	{
		print("Modif allow_range: %d->%d\r\n", Pid_allow_range, atoi(param));
		Pid_allow_range = atoi(param);
	}
	else if (strcmp(fun, "leave_dist") == 0)
	{
		print("Modif leave_dist: %d->%d\r\n", Leave_car_dist, atoi(param));
		Leave_car_dist = atoi(param);
	}
	else if(strcmp(fun,"round_test")==0){
		b_round_test_end = false;
		if (0 != aos_task_new("round_test", xp_robot_round_test, NULL, 1024)) {
			printf("create round_test thread fail");
			return -1;
		}
	}
	else if(strcmp(fun,"round_exit")==0){
		b_round_test_end = true;
		aos_task_delete("round_test");
	}
	else if(strcmp(fun,"org_x2")==0){
		org_x2 = atoi(param);
		println("org_x2 = %d", org_x2);
	}
	else if(strcmp(fun,"org_y2")==0){
		org_y2 = atoi(param);
		println("org_y2 = %d", org_y2);
	}
	else if(strcmp(fun,"org_x1")==0){
		org_x1 = atoi(param);
		println("org_x1 = %d", org_x1);
	}
	else if(strcmp(fun,"org_y1")==0){
		org_y1 = atoi(param);
		println("org_y1 = %d", org_y1);
	}
	else if(strcmp(fun,"adjust_test")==0){
		xp_robot_adjust_pose_by_lidar_time(60000);
	}
}
